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