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 run_experiment(tag,translation,params,Fzmin=-50.0,viewer=False):
env = Environment()
if viewer:
(env,options)=openhubo.setup('qtcoin')
env.SetDebugLevel(3)
time.sleep(.25)
[robot,ctrl,ind,ref]=openhubo.load_rlhuboplus(env,'footcontact.env.xml',True)
plate=env.GetKinBody('plate')
translate_body(plate,translation)
setup_sensors(robot)
[data,params]=sinusoidal_rock(robot,*params)
lcop_x=[]
lcop_y=[]
rcop_x=[]
rcop_y=[]
for k in range(size(data,1)):
#Arbitrary cutoff to reject light contact...this could be augmented with sensor data, knowing overall body accelerations etc.
if data[2,k]<Fzmin:
lcop_x.append(-data[1,k]/data[2,k])
lcop_y.append(-data[0,k]/data[2,k])
if data[5,k]<Fzmin:
rcop_x.append(-data[4,k]/data[5,k])
rcop_y.append(-data[3,k]/data[5,k])
filename="contact_{}_{}.pickle".format(tag,datetime.datetime.now().strftime("%Y_%m_%d_%H_%M_%S"))
with open(filename,'w') as f:
pickle.dump([data,params,lcop_x,lcop_y,rcop_x,rcop_y,translation],f)
env.Destroy()
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):
(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)
def run_experiment(tag, translation, params, Fzmin=-50.0, viewer=False):
env = Environment()
if viewer:
(env, options) = openhubo.setup('qtcoin')
env.SetDebugLevel(3)
time.sleep(.25)
[robot, ctrl, ind,
ref] = openhubo.load_rlhuboplus(env, 'footcontact.env.xml', True)
plate = env.GetKinBody('plate')
translate_body(plate, translation)
setup_sensors(robot)
[data, params] = sinusoidal_rock(robot, *params)
lcop_x = []
lcop_y = []
rcop_x = []
rcop_y = []
for k in range(size(data, 1)):
#Arbitrary cutoff to reject light contact...this could be augmented with sensor data, knowing overall body accelerations etc.
if data[2, k] < Fzmin:
lcop_x.append(-data[1, k] / data[2, k])
lcop_y.append(-data[0, k] / data[2, k])
if data[5, k] < Fzmin:
rcop_x.append(-data[4, k] / data[5, k])
rcop_y.append(-data[3, k] / data[5, k])
filename = "contact_{}_{}.pickle".format(
tag,
datetime.datetime.now().strftime("%Y_%m_%d_%H_%M_%S"))
with open(filename, 'w') as f:
pickle.dump(
[data, params, lcop_x, lcop_y, rcop_x, rcop_y, translation], f)
env.Destroy()
def get_fingers_mask(joints, mask):
return [j for j in joints if _re.search(mask, j.GetName())]
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)
else:
if force[2]<1 and force[2]>-1:
force[2]=sign(force[2])
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)
#//
#// This program is distributed in the hope that it will be useful,
#// but WITHOUT ANY WARRANTY; without even the implied warranty of
#// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
#// GNU Lesser General Public License for more details.
#//
#// 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()
"""Trajectory Control example for hubo+ model."""
import openhubo as oh
from openravepy import planningutils
from openhubo import trajectory
from numpy import pi
[env, options] = oh.setup('qtcoin')
env.SetDebugLevel(4)
options.physics = True
options.ghost = True
[robot, ctrl, ind, ref, recorder] = oh.load_scene(env, options)
env.StartSimulation(oh.TIMESTEP)
#Initialize pose object and trajectory for robot
pose = oh.Pose(robot, ctrl)
[traj, config] = trajectory.create_trajectory(robot)
trajectory.traj_append(traj, pose.to_waypt(0.01))
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
""" Simple test script to run some of the functions above. """ # 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
#Only return valid finger joints:
if not left_right:
return [robot.GetJoint(n) for n in FINGER_NAMES if robot.GetJoint(n) is not None]
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)
#// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
#// GNU Lesser General Public License for more details.
#//
#// 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'
import openravepy as _rave
import openhubo as oh
import time
from numpy import pi, sqrt
(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
options.scenefile = None
[robot, ctrl, ind, ref, recorder] = oh.load_scene(env, options)
#Change the pose to lift the elbows and send
pose = oh.Pose(robot, ctrl)
pose['REP'] = -pi / 2
pose['LEP'] = -pi / 2
pose.send()
#!/usr/bin/env python
from __future__ import with_statement # for python 2.5
__author__ = 'Robert Ellenberg'
__license__ = 'GPLv3 license'
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()
T = comps.Transform(link.GetTransform())
print T
if re.search("post", link.GetName()):
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))
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 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()
# // 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"
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)
"""Trajectory Control example for hubo+ model."""
import openhubo as oh
from openravepy import planningutils
from openhubo import trajectory
from numpy import pi
[env,options]=oh.setup('qtcoin')
env.SetDebugLevel(4)
options.physics=True
options.ghost=True
[robot,ctrl,ind,ref,recorder]=oh.load_scene(env,options)
env.StartSimulation(oh.TIMESTEP)
#Initialize pose object and trajectory for robot
pose=oh.Pose(robot,ctrl)
[traj,config]=trajectory.create_trajectory(robot)
trajectory.traj_append(traj,pose.to_waypt(0.01))
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
