def close_finger(finger, offset, tol=.01):
"""Close a finger joint by up to a given amount, checking for collision.
Note that this implementation is slow...
"""
link = finger.GetHierarchyChildLink()
#Make a pose for the robot
pose = _oh.Pose(finger.GetParent())
initial = pose[finger]
env = finger.GetParent().GetEnv()
report = CollisionReport()
step = offset
while abs(step) > tol:
pose[finger] += step
pose.send()
collision = env.CheckCollision(link, report=report)
if collision:
pose[finger] -= step
pose.send()
step /= 2.
elif abs(pose[finger] - initial) >= offset:
#Met or exceeded desired offset without collision
break
return offset
def test_anchors(self):
raveLogInfo('Loaded {}'.format(filename))
pose = openhubo.Pose(self.robot)
errors_home = check_all_constraints(pose)
pose['RSR'] = 15. * pi / 180
pose['LSR'] = -15. * pi / 180
errors_bent_SR = check_all_constraints(pose)
pose['REP'] = 10. * pi / 180
pose['LEP'] = 10. * pi / 180
errors_bent_EP = check_all_constraints(pose)
#raveLogDebug( "Error sums for {}:".format(self.robot.GetName()))
self.assertLess(
min([
errmax(errors_home),
errmax(errors_bent_SR),
errmax(errors_bent_EP)
]), 1e-10)
def test_hubo_read_trajectory(self):
robot = self.robot
pose = oh.Pose(robot)
pose.update(mapping.create_random_bounded_pose(robot))
traj, config = trajectory.create_trajectory(robot, pose.to_waypt())
print traj
trajectory.write_hubo_traj(traj, robot, .04)
def read_swarthmore_traj(filename,robot,dt=.01,retime=True):
""" Read in trajectory data stored in Swarthmore's format
(data by row, single space separated)
"""
#Setup trajectory and source file
[traj,config]=create_trajectory(robot)
f=open(filename,'r')
#Read in blank header row
f.readline().rstrip()
#Hard code names based on guess from DoorOpen.txt
names=['WST','LSP','LSR','LSY','LEP','LWY','LWP']
signs=[-1,1,-1,-1,1,-1,1]
offsets=[0,0,15.0,0,0,0,0]
#names=['WST','RSP','RSR','RSY','REP','RWY','RWP']
pose=_oh.Pose(robot)
for line in f:
jointvals=[float(x) for x in line.rstrip().split(' ')]
for (i,n) in enumerate(names):
pose[n]=(jointvals[i]-offsets[i])*pi/180*signs[i]
traj_append(traj,pose.to_waypt(dt))
if retime:
print _rave.planningutils.RetimeActiveDOFTrajectory(traj,robot,True)
f.close()
return traj
def close_huboplus_hand(robot, index=0, angle=pi / 2, weights=None):
manip = robot.GetManipulators()[index]
fingers = manip.GetChildDOFIndices()
pose = _oh.Pose(robot)
if weights is None:
weights = _np.ones(len(fingers))
for i, k in enumerate(fingers):
pose[k] += angle * weights[i]
pose.send()
def to_openrave(self,dt=None,retime=True,clip=True):
#Assumes that ALL joints are specified for now
[traj,config]=create_trajectory(self.robot)
#print config.GetDOF()
with self.robot:
T0=self.base.GetTransform()
print T0
pose=_oh.Pose(self.robot)
(lower,upper)=self.robot.GetDOFLimits()
for k in xrange(_np.size(self.srcdata,0)):
#for k in xrange(3):
#print T[0:3,3]
pose_map={key:self.srcdata[k,v] for (key,v) in self.joint_map.items()}
pose_array=array([pose_map[dof] if pose_map.has_key(dof) else 0.0 for dof in xrange(self.robot.GetDOF())])
pose.values=pose_array*self.joint_signs+self.joint_offsets
pose.send(True)
T=self.get_transform(T0,self.srcdata[k,0:6])
if clip:
#oldvals=pose.values
pose.values=_np.maximum(pose.values,lower*.999)
pose.values=_np.minimum(pose.values,upper*.999)
#err = pose.values-oldvals
#if sum(abs(err))>0:
#print k,err
#print pose.values
#Note this method does not use a controller
aff = _rave.RaveGetAffineDOFValuesFromTransform(T,_rave.DOFAffine.Transform)
if dt<0 or dt is None:
dt=self.timestep
traj_append(traj,pose.to_waypt(dt,aff))
print "retiming? {}".format(retime)
if retime:
dummy_limits=_np.ones(self.robot.GetDOF()+7)*100
_rave.planningutils.RetimeAffineTrajectory(traj,dummy_limits,dummy_limits,hastimestamps=True)
#_rave.planningutils.RetimeActiveDOFTrajectory(traj,self.robot,True)
#Store locally because why not
self.traj=traj
return traj
def read_youngbum_traj(filename,robot,dt=.01,scale=1.0,retime=True):
""" Read in trajectory data stored in Youngbum's format (100Hz data):
HPY LHY LHR ... RWP (3-letter names)
+ - + ... + (sign of joint about equivalent global axis + / -)
0.0 5.0 2.0 ... -2.0 (Offset of joint from openHubo "zero" in YOUR sign convention)
(data by row, single space separated)
"""
#TODO: handle multiple spaces
#Setup trajectory and source file
[traj,config]=create_trajectory(robot)
pose=_oh.Pose(robot)
f=open(filename,'r')
#Read in header row to find joint names
header=f.readline().rstrip()
names=header.split(' ')
#Read in sign row
signlist=f.readline().rstrip().split(' ')
signs=[]
for s in signlist:
if s == '+':
signs.append(1)
else:
signs.append(-1)
#Read in offset row (fill with _np.zeros if not used)
offsetlist=f.readline().rstrip().split(' ')
offsets=[float(x) for x in offsetlist]
k=0
for line in f:
jointvals=[float(x) for x in line.rstrip().split(' ')]
for (i,n) in enumerate(names):
pose[n]=(jointvals[i]+offsets[i])*pi/180.0*signs[i]*scale
traj.Insert(k,pose.to_waypt(dt))
k=k+1
if retime:
_rave.planningutils.RetimeActiveDOFTrajectory(traj,robot,True)
return traj
(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
#Choose degrees of freedom that are allowed to be moved to explore towards
#the goal.
activedofs = first_pose.ActivateManipsByIndex(robot, [0, 1])
activedofs.append(ind('LHP'))
activedofs.append(ind('RHP'))
#Set the goal pose as
for x in activedofs:
first_pose.jointgoals.append(pose[x])
#Add in hip pitches
# 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 pose1['LHP']=-pi/8 pose1['RHP']=-pi/8
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()
physics = env.GetPhysicsEngine()
physics.SetGravity([0, 0, 0])
ctrl2 = _rave.RaveCreateController(env, "idealcontroller")
robot.SetController(ctrl2)
env.StartSimulation(timestep=oh.TIMESTEP)
initialPose = robot.GetDOFValues()
time.sleep(5)
finalPose = robot.GetDOFValues()
err = sqrt(sum(pow(initialPose - finalPose, 2)))
[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
pose['LEP'] = -pi / 4
pose['RSP'] = -pi / 4
pose['LSP'] = -pi / 4
swing_and_measure(pose)
pose['REP'] = 0
pose['LEP'] = 0
pose['RSP'] = 0
pose['LSP'] = 0
swing_and_measure(pose)
env.StartSimulation(openhubo.TIMESTEP)
def apply_torque(pose, mask, T):
"""Use Pose class regex function to easily (but slowly) assign torques to a slice of joints"""
pose.useregex = True
pose[mask] = T
(env, options) = oh.setup('qtcoin')
env.SetDebugLevel(3)
options.scenefile = 'gripper.env.xml'
options.robotfile = None
options.physics = 'ode'
options.stop = True
[robot, ctrl, ind, ref, recorder] = oh.load_scene(env, options)
rod = env.GetKinBody('rod')
trans = rod.GetTransform()
pose = oh.Pose(robot)
success = False
strength = 0
oh.set_robot_color(robot, [.7, .7, .7], [.7, .7, .7], 0.0)
oh.set_finger_torquemode(robot)
#recorder.realtime=False
#recorder.filename='griptest.avi'
#recorder.start()
while not success:
strength += .2
print "Added torque {}".format(strength)
rod.SetVelocity(zeros(3), zeros(3))
rod.SetTransform(trans)
def read_text_traj(filename,robot,dt=.01,scale=1.0):
""" Read in trajectory data stored in Youngbum's format (100Hz data):
HPY LHY LHR ... RWP (3-letter names)
+ - + ... + (sign of joint about equivalent global axis + / -)
0.0 5.0 2.0 ... -2.0 (Offset of joint from openHubo "zero" in YOUR sign
convention and scale)
1000 1000 pi/180 pi/180 ... pi/180 (scale of your units wrt openrave
default)
(data by row, single space separated)
"""
#TODO: handle multiple spaces
#Setup trajectory and source file
ind=_oh.make_name_to_index_converter(robot)
f=open(filename,'r')
#Read in header row to find joint names
header=f.readline().rstrip()
#print header.split(' ')
[traj,config]=create_trajectory(robot)
k=0
indices={}
Tindices={}
Tmap={'X':0,'Y':1,'Z':2,'R':3,'P':4,'W':5}
for s in header.split(' '):
j=ind(s)
if j>=0:
indices.setdefault(k,j)
try:
Tindices.setdefault(k,Tmap[s])
except KeyError:
pass
except:
raise
k=k+1
#Read in sign row
signlist=f.readline().rstrip().split(' ')
signs=[]
for s in signlist:
if s == '+':
signs.append(1)
else:
signs.append(-1)
#Read in offset row (fill with _np.zeros if not used)
offsetlist=f.readline().rstrip().split(' ')
offsets=[float(x) for x in offsetlist]
#Read in scale row (fill with _np.ones if not used)
scalelist=f.readline().rstrip().split(' ')
scales=[float(x) for x in scalelist]
pose=_oh.Pose(robot)
for line in f:
vals=[float(x) for x in line.rstrip().split(' ')]
Tdata=_np.zeros(6)
for i,v in enumerate(vals):
if indices.has_key(i):
pose[indices[i]]=(vals[i]+offsets[i])*scales[i]*signs[i]*scale
elif Tindices.has_key(i):
Tdata[Tindices[i]]=(vals[i]+offsets[i])*scales[i]*signs[i]*scale
#TODO: clip joint vals at limits
#TODO: use axis angle form for RPY data?
T=array(_comps.Transform.make_transform(Tdata[3:],Tdata[0:3]))
traj_append(traj,pose.to_waypt(dt,_rave.poseFromMatrix(T)))
return traj
