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