def test_timing(arm, config_params, osc_params, use_cython): robot_config = arm.Config(use_cython=use_cython, **config_params) ctrlr = OSC(robot_config, **osc_params) # then run for timing n_trials = 1000 times = np.zeros(n_trials + 1) for ii in range(n_trials + 1): q = np.random.random(robot_config.N_JOINTS) * 2 * np.pi dq = np.random.random(robot_config.N_JOINTS) * 5 target = np.random.random(6) * 2 - 1 start = timeit.default_timer() ctrlr.generate(q=q, dq=dq, target=target) times[ii] = timeit.default_timer() - start # strip off first loop to remove load time times = times[1:] average_time = np.sum(times) / n_trials return average_time
q_track = [] count = 0 link_name = "EE" try: while True: if interface.viewer.exit: glfw.destroy_window(interface.viewer.window) break feedback = interface.get_feedback() hand_xyz = robot_config.Tx(link_name) u = ctrlr.generate( q=feedback["q"], dq=feedback["dq"], target=target, ref_frame=link_name, ) interface.send_forces(u) if np.linalg.norm(hand_xyz[:N_JOINTS] - target[:N_JOINTS]) < 0.01: target[0] = np.random.uniform(0.2, 0.25) * np.sign( np.random.uniform(-1, 1)) target[1] = np.random.uniform(0.2, 0.25) * np.sign( np.random.uniform(-1, 1)) target[2] = np.random.uniform(0.4, 0.5) interface.set_mocap_xyz("target", target[:3]) q_track.append(feedback["q"]) count += 1
def run_test(self, n_neurons=1000, n_ensembles=1, decimal_scale=1, test_name="adaptive_training", session=None, run=None, weights_file=None, pes_learning_rate=1e-6, backend=None, autoload=False, time_limit=30, target_type='single', offset=None, avoid_limits=False, additional_mass=0, kp=20, kv=6, ki=0, integrate_err=False, ee_adaptation=False, joint_adaptation=False, simulate_wear=False, probe_weights=False, seed=None, friction_bootstrap=False, redis_adaptation=False, SCALES=None, MEANS=None): """ The test script used to collect data for training. The use of the adaptive controller and the type of backend can be specified. The script is also automated to use the correct weights files for continuous learning. The standard naming convention used is runs are consecutive tests where the previous learned weights are used. The whole of these runs are a single session. Multiple sessions of these runs can then be used for averaging and statictical purposes. The adaptive controller uses a 6 dimensional input and returns a 3 dimensional output. The base, shoulder and elbow joints' positions and velocities are used as the input, and the control signal for the corresponding joints gets returned. Parameters ---------- n_neurons: int, Optional (Default: 1000) the number of neurons in the adaptive population n_ensembles: int, Optional (Default: 1) the number of ensembles of n_neurons number of neurons decimal_scale: int, Optional (Default: 1) used for scaling the spinnaker input. Due to precision limit, spinnaker input gets scaled to allow for more decimals, then scaled back once extracted. Also can be used to account for learning speed since spinnaker runs in real time test_name: string, Optional (Default: "dewolf_2017_data") folder name where data is saved session: int, Optional (Default: None) The current session number, if left to None then it will be automatically updated based on the current session. If the next session is desired then this will need to be updated. run: int, Optional (Default: None) The current nth run that specifies to use the weights from the n-1 run. If set to None if will automatically increment based off the last saved weights. weights_file: string, Optional (Default: None) the path to the desired saved weights to use. If None will automatically take the most recent weights saved in the 'test_name' directory pes_learning_rate: float, Optional (Default: 1e-6) the learning rate for the adaptive population backend: string None: non adaptive control, Optional (Default: None) 'nengo': use nengo as the backend for the adaptive population 'spinnaker': use spinnaker as the adaptive population autoload: boolean, Optional (Default: False) True: used the specified weights, or the last set of learned weights if not specified False: use the specified weights, if not specified start learning from zero time_limit: float, Optional (Default: 30) the time limit for each run in seconds target_type: string, Optional (Default: single) single: one target multi: five targets figure8: move in figure8 pattern vision: get target from vision system offset: float array, Optional (Default: None) Set the offset to the end effector if something other than the default is desired. Use the form [x_offset, y_offset, z_offset] avoid_limits: boolean, Optional (Default: False) set true if there are limits you would like to avoid additional_mass: float, Optional (Default: 0) any extra mass added to the EE if known [kg] kp: float, Optional (Default: 20) proportional gain term kv: float, Optional (Default: 6) derivative gain term ki: float, Optional (Default: 0) integral gain term integrate_err: Boolean, Optional (Default: False) True to add I term to PD control (PD -> PID) ee_adaptation: Booleanm Optional (Default: False) True if doing neural adaptation in task space joint_adaptation: Boolean Optional (Default: False) True if doing neural adaptation in joint space simulate_wear: Boolean Optional (Default: False) True to add a simulated wearing of the joints that mimics friction by opposing motion probe_weights: Boolean Optional (Default: False) True to probe adaptive population for decoders seed: int Optional, (Default: None) seed used for random number generation in adaptive population friction_bootstrap: Boolean Optional (Default: False) True if want to pass an estimate of the friction signal as starting point for adaptive population redis_adaptation: Boolean Optional (Default: False) True to send adaptive inputs to redis and read outputs back, allows user to use any backend they like as long as it can read/write from redis Attributes ---------- TARGET_XYZ: array of floats the goal target, used for calculating error target: array of floats [x, y, z, vx, vy, vz] the filtered target, used for calculating error for figure8 test since following trajectory and not moving to a single final point """ print("RUN PASSED IN IS: ", run) if redis_adaptation: try: import redis redis_server = redis.StrictRedis(host='localhost') except ImportError: print( "You must install redis to do adaptation through a redis" + " server") # Set the target based on the source and type of test PRESET_TARGET = np.array([[.57, 0.03, .87]]) if target_type == 'multi': PRESET_TARGET = np.array([[.56, -.09, .95], [.12, .15, .80], [.80, .26, .61], [.38, .46, .81], [.0, .0, 1.15]]) time_limit /= len(PRESET_TARGET) elif target_type == 'vision': # try to setup redis server if vision targets are desired try: import redis redis_server = redis.StrictRedis(host='localhost') self.redis_server = redis_server except ImportError: print( 'ERROR: Install redis to use vision targets, using preset targets' ) elif target_type == 'figure8': try: import pydmps except ImportError: print('\npydmps library required, see ' + 'https://github.com/studywolf/pydmps\n') # create a dmp that traces a figure8 x = np.linspace(0, np.pi * 2, 100) dmps_traj = np.array([0.2 * np.sin(x), 0.2 * np.sin(2 * x) + 0.7]) dmps = pydmps.DMPs_rhythmic(n_dmps=2, n_bfs=50, dt=0.001) dmps.imitate_path(dmps_traj) # get averages and scale of target locations for scaling into network x_avg = np.mean(PRESET_TARGET[:, 0]) y_avg = np.mean(PRESET_TARGET[:, 1]) z_avg = np.mean(PRESET_TARGET[:, 2]) x_scale = np.max(PRESET_TARGET[:, 0]) - np.min(PRESET_TARGET[:, 0]) y_scale = np.max(PRESET_TARGET[:, 1]) - np.min(PRESET_TARGET[:, 1]) z_scale = np.max(PRESET_TARGET[:, 2]) - np.min(PRESET_TARGET[:, 2]) # initialize our robot config robot_config = abr_jaco2.Config(use_cython=True, hand_attached=True, SCALES=SCALES, MEANS=MEANS) self.robot_config = robot_config zeros = np.zeros(robot_config.N_JOINTS) # get Jacobians to each link for calculating perturbation self.J_links = [ robot_config._calc_J('link%s' % ii, x=[0, 0, 0]) for ii in range(robot_config.N_LINKS) ] self.JEE = robot_config._calc_J('EE', x=[0, 0, 0]) # account for wrist to fingers offset R_func = robot_config._calc_R('EE') # Use user defined offset if one is specified if offset is None: OFFSET = robot_config.OFFSET else: OFFSET = offset robot_config.Tx('EE', q=zeros, x=OFFSET) # temporarily set backend to nengo if non adaptive if selected so we can # still use the weight saving function in dynamics_adaptation.py in the # abr_control repo if backend is None: adapt_backend = 'nengo' else: adapt_backend = backend # create our adaptive controller if ee_adaptation and joint_adaptation: n_input = 4 n_output = 3 elif ee_adaptation and not joint_adaptation: n_input = 3 n_output = 3 pes_learning_rate /= ki elif joint_adaptation and not ee_adaptation: n_input = 4 n_output = 2 else: n_input = 1 n_output = 1 # for use with weight estimation, is the number of joint angles being # passed to the adaptive controller (not including velocities) self.adapt_dim = n_output self.decimal_scale = decimal_scale # if user specifies an additional mass, use the estimation function to # give the adaptive controller a better starting point self.additional_mass = additional_mass if self.additional_mass != 0 and weights_file is None: # if the user knows about the mass at the EE, try and improve # our starting point self.fake_gravity = np.array( [[0, 0, -9.81 * self.additional_mass, 0, 0, 0]]).T print('Using mass estimate of %f kg as starting point' % self.additional_mass) adapt = signals.DynamicsAdaptation( n_input=n_input, n_output=n_output, n_neurons=n_neurons, n_ensembles=n_ensembles, pes_learning_rate=pes_learning_rate, intercepts=(-0.5, -0.2), weights_file=weights_file, backend=adapt_backend, session=session, run=run, test_name=test_name, autoload=autoload, function=self.gravity_estimate, probe_weights=probe_weights, seed=seed) elif friction_bootstrap and weights_file is None: # if the user knows about the friction, try and improve # our starting point print('Using friction estimate as starting point') adapt = signals.DynamicsAdaptation( n_input=n_input, n_output=n_output, n_neurons=n_neurons, n_ensembles=n_ensembles, pes_learning_rate=pes_learning_rate, intercepts=(-0.5, -0.2), weights_file=weights_file, backend=adapt_backend, session=session, run=run, test_name=test_name, autoload=autoload, function=lambda x: self.friction(x) * -1, probe_weights=probe_weights, seed=seed) else: adapt = signals.DynamicsAdaptation( n_input=n_input, n_output=n_output, n_neurons=n_neurons, n_ensembles=n_ensembles, pes_learning_rate=pes_learning_rate, intercepts=(-0.5, -0.2), weights_file=weights_file, backend=adapt_backend, session=session, run=run, test_name=test_name, autoload=autoload, probe_weights=probe_weights, seed=seed) # get save location of saved data [location, run_num] = adapt.weights_location(test_name=test_name, run=run, session=session) # if using integral term for PID control, check if previous weights # have been saved if integrate_err: if run_num < 1: run_num = 0 int_err_prev = [0, 0, 0] else: int_err_prev = np.squeeze( np.load(location + '/run%i_data/int_err%i.npz' % (run_num - 1, run_num - 1))['int_err'])[-1] else: int_err_prev = None # instantiate operational space controller print('using int err of: ', int_err_prev) ctrlr = OSC(robot_config, kp=kp, kv=kv, ki=ki, vmax=1, null_control=True, int_err=int_err_prev) # add joint avoidance controller if specified to do so if avoid_limits: avoid = signals.AvoidJointLimits( robot_config, # joint 4 flipped because does not cross 0-2pi line min_joint_angles=[None, 1.1, 0.5, 3.5, 2.0, 1.6], max_joint_angles=[None, 3.65, 6.25, 6.0, 5.0, 4.6], max_torque=[4] * robot_config.N_JOINTS, cross_zero=[True, False, False, False, True, False], gradient=[False, True, True, True, True, False]) # if not planning the trajectory (figure8 target) then use a second # order filter to smooth out the trajectory to target(s) if target_type != 'figure8': path = path_planners.SecondOrder(robot_config) n_timesteps = 4000 w = 1e4 / n_timesteps zeta = 2 dt = 0.003 # run controller once to generate functions / take care of overhead # outside of the main loop, because force mode auto-exits after 200ms ctrlr.generate(zeros, zeros, np.zeros(3), offset=OFFSET) interface = abr_jaco2.Interface(robot_config) # connect to and initialize the arm interface.connect() interface.init_position_mode() interface.send_target_angles(robot_config.INIT_TORQUE_POSITION) # set up lists for tracking data time_track = [] q_track = [] dq_track = [] u_track = [] adapt_track = [] error_track = [] training_track = [] target_track = [] ee_track = [] input_signal = [] if integrate_err: int_err_track = [] if simulate_wear: self.F_prev = np.array([0, 0]) friction_track = [] try: interface.init_force_mode() for ii in range(0, len(PRESET_TARGET)): # counter for print statements count = 0 # track loop_time for stopping test loop_time = 0 # get joint angle and velocity feedback feedback = interface.get_feedback() q = feedback['q'] dq = feedback['dq'] dq[abs(dq) < 0.05] = 0 # calculate end-effector position ee_xyz = robot_config.Tx('EE', q=q, x=OFFSET) # last three terms used as started point for target EE velocity target = np.concatenate((ee_xyz, np.array([0, 0, 0])), axis=0) # M A I N C O N T R O L L O O P while loop_time < time_limit: start = timeit.default_timer() prev_xyz = ee_xyz if target_type == 'vision': TARGET_XYZ = self.get_target_from_camera() TARGET_XYZ = self.normalize_target(TARGET_XYZ) target = path.step(y=target[:3], dy=target[3:], target=TARGET_XYZ, w=w, zeta=zeta, dt=dt, threshold=0.05) elif target_type == 'figure8': y, dy, ddy = dmps.step() target = [0.65, y[0], y[1], 0, dy[0], dy[1]] TARGET_XYZ = target[:3] else: TARGET_XYZ = PRESET_TARGET[ii] target = path.step(y=target[:3], dy=target[3:], target=TARGET_XYZ, w=w, zeta=zeta, dt=dt, threshold=0.05) # calculate euclidean distance to target error = np.sqrt(np.sum((ee_xyz - TARGET_XYZ)**2)) # get joint angle and velocity feedback feedback = interface.get_feedback() q = feedback['q'] dq = feedback['dq'] dq[abs(dq) < 0.05] = 0 # calculate end-effector position ee_xyz = robot_config.Tx('EE', q=q, x=OFFSET) if ee_adaptation and joint_adaptation: # adaptive control in state space training_signal = TARGET_XYZ - ee_xyz # calculate the adaptive control signal adapt_input = np.array([ robot_config.scaledown('q', q)[1], robot_config.scaledown('q', q)[2], robot_config.scaledown('dq', dq)[1], robot_config.scaledown('dq', dq)[2] ]) u_adapt = adapt.generate( input_signal=adapt_input, training_signal=training_signal) u_adapt /= decimal_scale elif ee_adaptation and not joint_adaptation: # adaptive control in state space training_signal = TARGET_XYZ - ee_xyz # scale inputs adapt_input = np.array([(ee_xyz[0] - x_avg) / x_scale, (ee_xyz[1] - y_avg) / y_scale, (ee_xyz[2] - z_avg) / z_scale]) u_adapt = adapt.generate( input_signal=adapt_input, training_signal=training_signal) u_adapt /= decimal_scale else: u_adapt = None # Need to create controller here, in case using ee # adaptation, it needs to be passed to the OSC controller # calculate the base operation space control signal u_base = ctrlr.generate(q=q, dq=dq, target_pos=target[:3], target_vel=target[3:], offset=OFFSET, ee_adapt=u_adapt) # account for stiction in jaco2 base if u_base[0] > 0: u_base[0] *= 3.0 else: u_base[0] *= 2.0 if joint_adaptation and not ee_adaptation: training_signal = np.array([ ctrlr.training_signal[1], ctrlr.training_signal[2] ]) # calculate the adaptive control signal adapt_input = np.array([ robot_config.scaledown('q', q)[1], robot_config.scaledown('q', q)[2], robot_config.scaledown('dq', dq)[1], robot_config.scaledown('dq', dq)[2] ]) u_adapt = adapt.generate( input_signal=adapt_input, training_signal=training_signal) # add adaptive signal to base controller u_adapt = np.array([ 0, u_adapt[0] / decimal_scale, u_adapt[1] / decimal_scale, 0, 0, 0 ]) u = u_base + u_adapt elif redis_adaptation: training_signal = np.array([ ctrlr.training_signal[1], ctrlr.training_signal[2] ]) adapt_input = np.array([ robot_config.scaledown('q', q)[1], robot_config.scaledown('q', q)[2], robot_config.scaledown('dq', dq)[1], robot_config.scaledown('dq', dq)[2] ]) # send input information to redis redis_server.set( 'input_signal', '%.3f %.3f %.3f %.3f' % (adapt_input[0], adapt_input[1], adapt_input[2], adapt_input[3])) redis_server.set( 'training_signal', '%.3f %.3f' % (training_signal[0], training_signal[1])) # get adaptive output back u_adapt = redis_server.get('u_adapt').decode('ascii') u_adapt = np.array( [float(val) for val in u_adapt.split()]) u_adapt = np.array( [0, u_adapt[0], u_adapt[1], 0, 0, 0]) u = u_base + u_adapt else: u = u_base u_adapt = None training_signal = np.zeros(3) adapt_input = np.zeros(3) # add limit avoidance if True if avoid_limits: u += avoid.generate(q) if simulate_wear: u_friction = self.friction(dq[1:3]) u += [0, u_friction[0], u_friction[1], 0, 0, 0] friction_track.append(np.copy(u_friction)) # send forces interface.send_forces(np.array(u, dtype='float32')) # track data q_track.append(np.copy(q)) dq_track.append(np.copy(dq)) u_track.append(np.copy(u)) adapt_track.append(np.copy(u_adapt)) error_track.append(np.copy(error)) training_track.append(np.copy(training_signal)) end = timeit.default_timer() - start loop_time += end time_track.append(np.copy(end)) target_track.append(np.copy(TARGET_XYZ)) input_signal.append(np.copy(adapt_input)) ee_track.append(np.copy(ee_xyz)) if integrate_err: int_err_track.append(np.copy(ctrlr.int_err)) if count % 1000 == 0: print('error: ', error) print('dt: ', end) #print('adapt: ', u_adapt) #print('int_err/ki: ', ctrlr.int_err) #print('q: ', q) #print('hand: ', ee_xyz) #print('target: ', target) #print('control: ', u_base) count += 1 except: print(traceback.format_exc()) finally: # close the connection to the arm interface.init_position_mode() print("RUN IN IS: ", run) # get save location of weights to save tracked data in same directory [location, run_num] = adapt.weights_location(test_name=test_name, run=run, session=session) print("RUN OUT IS: ", run_num) if backend != None: run_num += 1 # Save the learned weights adapt.save_weights(test_name=test_name, session=session, run=run) interface.send_target_angles(robot_config.INIT_TORQUE_POSITION) interface.disconnect() print('**** RUN STATS ****') print('Average loop speed: ', sum(time_track) / len(time_track)) print('AVG Error/Step: ', sum(error_track) / len(error_track)) print('Run number ', run_num) print('Saving tracked data to ', location + '/run%i_data' % (run_num)) print('*******************') time_track = np.array(time_track) q_track = np.array(q_track) u_track = np.array(u_track) adapt_track = np.array(adapt_track) error_track = np.array(error_track) training_track = np.array(training_track) input_signal = np.array(input_signal) ee_track = np.array(ee_track) dq_track = np.array(dq_track) if integrate_err: int_err_track = np.array(int_err_track) if simulate_wear: friction_track = np.array(friction_track) if not os.path.exists(location + '/run%i_data' % (run_num)): os.makedirs(location + '/run%i_data' % (run_num)) np.savez_compressed(location + '/run%i_data/q%i' % (run_num, run_num), q=[q_track]) np.savez_compressed(location + '/run%i_data/time%i' % (run_num, run_num), time=[time_track]) np.savez_compressed(location + '/run%i_data/u%i' % (run_num, run_num), u=[u_track]) np.savez_compressed(location + '/run%i_data/adapt%i' % (run_num, run_num), adapt=[adapt_track]) np.savez_compressed(location + '/run%i_data/target%i' % (run_num, run_num), target=[target_track]) np.savez_compressed(location + '/run%i_data/error%i' % (run_num, run_num), error=[error_track]) np.savez_compressed(location + '/run%i_data/training%i' % (run_num, run_num), training=[training_track]) np.savez_compressed(location + '/run%i_data/input_signal%i' % (run_num, run_num), input_signal=[input_signal]) np.savez_compressed(location + '/run%i_data/ee_xyz%i' % (run_num, run_num), ee_xyz=[ee_track]) np.savez_compressed(location + '/run%i_data/dq%i' % (run_num, run_num), dq=[dq_track]) if probe_weights: np.savez_compressed( location + '/run%i_data/probe%i' % (run_num, run_num), probe=[adapt.sim.data[adapt.nengo_model.weights_probe]]) if integrate_err: np.savez_compressed(location + '/run%i_data/int_err%i' % (run_num, run_num), int_err=[int_err_track]) if simulate_wear: np.savez_compressed(location + '/run%i_data/friction%i' % (run_num, run_num), friction=[friction_track]) if backend != 'nengo_spinnaker': # give user time to pause before the next run starts, only # works if looping through tests using a bash script import time print('2 Seconds to pause: Hit ctrl z to pause, fg to resume') time.sleep(1) print('1 Second to pause') time.sleep(1) print('Starting next test...')
for function in pos_path ] target_velocity = [ function(min(path_planner.time_to_converge, time_elapsed)) for function in vel_path ] else: next_target = path_planner.next() target = next_target[:3] target_velocity = next_target[3:] # generate an operational space control signal u = ctrlr.generate( q=feedback["q"], dq=feedback["dq"], target=np.hstack([target, np.zeros(3)]), target_velocity=np.hstack([target_velocity, np.zeros(3)]), ) # apply the control signal, step the sim forward interface.send_forces(u, update_display=(count % 20 == 0)) count += 1 time_elapsed += timeit.default_timer() - start finally: # stop and reset the simulation interface.disconnect() print("Simulation terminated...")
[np.random.random() * 2 - 1, np.random.random() * 2 + 1, 0]) # update the position of the target interface.set_target(target_xyz) path_planner.generate_path(state=hand_xyz, target=target_xyz, n_timesteps=n_timesteps, plot=False) # returns desired [position, velocity] target = path_planner.next_target() # generate an operational space control signal u = ctrlr.generate( q=feedback['q'], dq=feedback['dq'], target_pos=target[:3], # (x, y, z) target_vel=target[3:]) # (dx, dy, dz) # apply the control signal, step the sim forward interface.send_forces(u) # track data ee_path.append(np.copy(hand_xyz)) target_path.append(np.copy(target_xyz)) count += 1 finally: # stop and reset the simulation interface.disconnect()
import numpy as np import traceback import timeit import abr_jaco2 from abr_control.controllers import OSC # initialize our robot config robot_config = abr_jaco2.Config(use_cython=True) ctrlr = OSC(robot_config, kp=20, kv=10) # run controller once to generate functions / take care of overhead # outside of the main loop, because force mode auto-exits after 200ms zeros = np.zeros(robot_config.N_JOINTS) ctrlr.generate(zeros, zeros, np.zeros(6)) # create our interface for the jaco2 interface = abr_jaco2.Interface(robot_config) time_track = [] # connect to the jaco interface.connect() interface.init_position_mode() # Move to home position interface.send_target_angles(robot_config.START_ANGLES) try: print('Running loop speed test for the next 10 seconds...')
# create an obstacle interface.add_circle(xyz=[0, 0, 0], radius=.2) # create a target target_xyz = [0, 2, 0] interface.set_target(target_xyz) # set up lists for tracking data ee_path = [] target_path = [] try: # run ctrl.generate once to load all functions zeros = np.zeros(robot_config.N_JOINTS) ctrlr.generate(q=zeros, dq=zeros, target_pos=target_xyz) robot_config.orientation('EE', q=zeros) print('\nSimulation starting...\n') print('\nClick to move the obstacle.\n') while 1: # get arm feedback feedback = interface.get_feedback() hand_xyz = robot_config.Tx('EE', feedback['q']) # generate an operational space control signal u = ctrlr.generate(q=feedback['q'], dq=feedback['dq'], target_pos=target_xyz) # add in obstacle avoidance obs_xy = interface.get_mousexy()
pregenerated_path=pos_path, time_limit=run_time) vel_path = path_planner.convert_to_time( pregenerated_path=vel_path, time_limit=run_time) # get next target along trajectory if use_wall_clock: target = [function(time_elapsed) for function in pos_path] target_vel = [function(time_elapsed) for function in vel_path] else: target, target_vel = path_planner.next() interface.set_mocap_xyz('path_planner', target) # generate an operational space control signal u = ctrlr.generate( q=feedback['q'], dq=feedback['dq'], target=np.hstack((target, np.zeros(3))), #target_vel=np.hstack((target_vel, np.zeros(3))), ref_frame=link_name) # apply the control signal, step the sim forward interface.send_forces(u) if count % 500 == 0: print('ee_xyz: ', hand_xyz) print('target_xyz', target_xyz) ee_track.append(np.copy(hand_xyz)) target_track.append(interface.get_mocap_xyz(link_name)) count += 1 time_elapsed += timeit.default_timer() - start
# create our interface interface = PyGame(robot_config, arm_sim, dt=dt) interface.connect() # set up lists for tracking data ee_path = [] target_path = [] pregenerate_path = False print('\nPregenerating path to follow: ', pregenerate_path, '\n') try: # run ctrl.generate once to load all functions zeros_task = np.zeros(3) zeros_joint = np.zeros(robot_config.N_JOINTS) ctrlr.generate(q=zeros_joint, dq=zeros_joint, target_pos=zeros_task, target_vel=zeros_task) robot_config.R('EE', q=zeros_joint) print('\nSimulation starting...\n') print('\nClick to move the target.\n') count = 0 while 1: # get arm feedback feedback = interface.get_feedback() hand_xyz = robot_config.Tx('EE', feedback['q']) if count % n_timesteps == 0: target_xyz = np.array([ np.random.random() * 2 - 1, np.random.random() * 2 + 1,
while at_target < 500: if count > 5000: break filtered_target = path_planner.next() interface.set_mocap_xyz("target_orientation", filtered_target[:3]) feedback = interface.get_feedback() hand_xyz = robot_config.Tx("EE", feedback["q"]) if interface.viewer.exit: glfw.destroy_window(interface.viewer.window) break u = ctrlr.generate( q=feedback["q"], dq=feedback["dq"], target=filtered_target, ) # add gripper forces u = np.hstack((u, np.zeros(robot_config.N_GRIPPER_JOINTS))) # apply the control signal, step the sim forward interface.send_forces(u) # track data ee_track.append(np.copy(hand_xyz)) target_track.append(np.copy(pos_target[:3])) if np.linalg.norm(hand_xyz - pos_target) < 0.02: at_target += 1
# instantiate controller ctrlr = OSC(robot_config, kp=30, kv=20, vmax=None, null_controllers=null_controllers) # instantiate path planner and set parameters path = path_planners.SecondOrderFilter( n_timesteps=3000, w=1e4, zeta=2, threshold=0.05) # run controller once to generate functions / take care of overhead # outside of the main loop, because force mode auto-exits after 200ms zeros = np.zeros(robot_config.N_JOINTS) robot_config.Tx('EE', q=zeros) ctrlr.generate(zeros, zeros, zeros) interface = abr_jaco2.Interface(robot_config) target_xyz = np.array([.57, 0.03, .87]) time_limit = 30 # in seconds # create our adaptive controller adapt = signals.DynamicsAdaptation( n_input=4, n_output=2, n_neurons=1000, pes_learning_rate=1e-4, means=[3.14, 3.14, 0.05, 0.05], variances=[3.14, 3.14, 1, 1],
interface = PyGame(robot_config, arm_sim, dt=.001, on_click=on_click) interface.connect() # create a target feedback = interface.get_feedback() target_xyz = robot_config.Tx('EE', feedback['q']) interface.set_target(target_xyz) # set up lists for tracking data ee_path = [] target_path = [] try: # run ctrl.generate once to load all functions zeros = np.zeros(robot_config.N_JOINTS) ctrlr.generate(q=zeros, dq=zeros, target_pos=np.zeros(3)) robot_config.R('EE', q=zeros) print('\nSimulation starting...\n') print('\nClick to move the target.\n') count = 0 while 1: # get arm feedback feedback = interface.get_feedback() hand_xyz = robot_config.Tx('EE', feedback['q']) # generate an operational space control signal u = ctrlr.generate(q=feedback['q'], dq=feedback['dq'], target_pos=target_xyz,
robot_config = abr_jaco2.Config(use_cython=True, hand_attached=True) # instantiate operation space controller null_controllers = [] from abr_control.controllers import Damping null_controllers.append(Damping(robot_config=robot_config, kv=1)) ctrlr = OSC(robot_config, kp=30, kv=20, ki=0.002, null_controllers=null_controllers) # run controller once to generate functions / take care of overhead # outside of the main loop, because force mode auto-exits after 200ms zeros = np.zeros(robot_config.N_JOINTS) ctrlr.generate(zeros, zeros, zeros) # offset to move control point from palm to fingers robot_config.Tx('EE', q=zeros) # create our interface for the jaco2 interface = abr_jaco2.Interface(robot_config) target_xyz = np.array([[.56, -.09, .52], [.12, .15, .75], [.60, .26, .61], [.38, .46, .81]]) # connect to the jaco interface.connect() interface.init_position_mode() interface.send_target_angles(robot_config.INIT_TORQUE_POSITION) # set up arrays for tracking end-effector and target position error_track = []
target_pos, target_velocity = path._step(position=target_pos, velocity=target_velocity, target_position=target_xyz) target = np.hstack((target_pos, target_angles)) if count % 3000 == 0: target_xyz[2] = target_xyz[2] + -1 * shift shift *= -1 # ii = 3 # target_angles[-ii] = (target_angles[-ii] + 0.2) % (2*np.pi) u = ctrlr.generate(q=feedback['q'], dq=feedback['dq'], target=target, target_velocity=np.hstack( (target_velocity, np.zeros(3)))) if count % 1000 == 0: print(u) # apply the control signal, step the sim forward interface.send_forces(np.array(u, dtype='float32')) # track data ee_track.append(np.copy(hand_xyz)) ee_angles_track.append( transformations.euler_from_matrix( robot_config.R('EE', feedback['q']))) target_track.append(np.copy(target[:3])) target_angles_track.append(np.copy(target[3:]))
# update the position of the target interface.set_target(target_xyz) path_planner.generate_path( state=hand_xyz, target=target_xyz, # n_timesteps=n_timesteps, dx=0.01, plot=False) # returns desired [position, velocity] target = path_planner.next_target() # generate an operational space control signal u = ctrlr.generate( q=feedback['q'], dq=feedback['dq'], target_pos=target[:3], target_vel=target[3:], ) # apply the control signal, step the sim forward interface.send_forces(u, update_display=True if count % 20 == 0 else False) count += 1 finally: # stop and reset the simulation interface.disconnect() print('Simulation terminated...')
'target', transformations.euler_from_quaternion(target_orientation, axes='rxyz')) print('\nSimulation starting...\n') while 1: # get arm feedback feedback = interface.get_feedback() hand_xyz = robot_config.Tx('EE', feedback['q']) pos, vel = traj_planner.next()[:3] orient = orientation_planner.next() target = np.hstack([pos, orient]) u = ctrlr.generate( q=feedback['q'], dq=feedback['dq'], target=target, #target_vel=np.hstack([vel, np.zeros(3)]) ) # apply the control signal, step the sim forward interface.send_forces(u) # track data ee_track.append(np.copy(hand_xyz)) ee_angles_track.append( transformations.euler_from_matrix(robot_config.R( 'EE', feedback['q']), axes='rxyz')) target_track.append(np.copy(target[:3])) target_angles_track.append(np.copy(target[3:])) count += 1
pos_path = path_planner.convert_to_time( pregenerated_path=pos_path, time_limit=run_time) vel_path = path_planner.convert_to_time( pregenerated_path=vel_path, time_limit=run_time) # get next target along trajectory if use_wall_clock: target = [function(time_elapsed) for function in pos_path] target_vel = [function(time_elapsed) for function in vel_path] else: target, target_vel = path_planner.next() # generate an operational space control signal u = ctrlr.generate( q=feedback['q'], dq=feedback['dq'], target=np.hstack((target, np.zeros(3))), target_vel=np.hstack((target_vel, np.zeros(3))), ) # apply the control signal, step the sim forward interface.send_forces(u, update_display=(count % 20 == 0)) count += 1 time_elapsed += timeit.default_timer() - start finally: # stop and reset the simulation interface.disconnect() print('Simulation terminated...')
q_track = [] count = 0 link_name = 'EE' try: while True: if interface.viewer.exit: glfw.destroy_window(interface.viewer.window) break feedback = interface.get_feedback() hand_xyz = robot_config.Tx(link_name) u = ctrlr.generate( q=feedback['q'], dq=feedback['dq'], target=target, ref_frame=link_name, ) interface.send_forces(u) if count % 500 == 0: print('ee_xyz: ', hand_xyz) print('target', target) if np.linalg.norm(hand_xyz[:N_JOINTS] - target[:N_JOINTS]) < 0.01: target[0] = (np.random.uniform(0.2, 0.3) * np.sign(np.random.uniform(-1, 1))) target[1] = (np.random.uniform(0.2, 0.25) * np.sign(np.random.uniform(-1, 1))) target[2] = np.random.uniform(0.3, 0.4) interface.set_mocap_xyz('target', target[:3])
target_xyz = start + np.array([0.2, -0.2, 0.0]) interface.set_xyz(name='target', xyz=target_xyz) count = 0.0 while 1: # get joint angle and velocity feedback feedback = interface.get_feedback() target = np.hstack( [interface.get_xyz('target'), interface.get_orientation('target')]) # calculate the control signal u = ctrlr.generate( q=feedback['q'], dq=feedback['dq'], target=target, ) u_adapt = np.zeros(robot_config.N_JOINTS) u_adapt[1:3] = adapt.generate( input_signal=np.array([feedback['q'][1], feedback['q'][2]]), training_signal=np.array( [ctrlr.training_signal[1], ctrlr.training_signal[2]])) u += u_adapt # add an additional force for the controller to adapt to extra_gravity = robot_config.g(feedback['q']) * 2 u += extra_gravity # send forces into VREP, step the sim forward
# get next target along trajectory if use_wall_clock: target = [function(min(time_elapsed, run_time)) for function in pos_path] target_velocity = [ function(min(time_elapsed, run_time)) for function in vel_path ] else: next_target = path_planner.next() target = next_target[:3] target_velocity = next_target[3:] interface.set_mocap_xyz("path_planner", target) # generate an operational space control signal u = ctrlr.generate( q=feedback["q"], dq=feedback["dq"], target=np.hstack((target, np.zeros(3))), ref_frame=link_name, ) # apply the control signal, step the sim forward interface.send_forces(u) ee_track.append(np.copy(hand_xyz)) target_track.append(interface.get_xyz(link_name)) count += 1 time_elapsed += timeit.default_timer() - start error = np.linalg.norm(hand_xyz - target_xyz) if error < 0.02: interface.sim.model.geom_rgba[target_geom_id] = green else:
try: print("\nSimulation starting...") print("Click to move the target.") print("Press space to turn on adaptation.\n") count = 0 while 1: # get arm feedback feedback = interface.get_feedback() hand_xyz = robot_config.Tx("EE", feedback["q"]) target = np.hstack([target_xyz, target_angles]) # generate an operational space control signal u = ctrlr.generate( q=feedback["q"], dq=feedback["dq"], target=target, ) # if adaptation is on (toggled with space bar) if interface.adaptation: u += adapt.generate(input_signal=feedback["q"], training_signal=ctrlr.training_signal) fake_gravity = np.array([[0, -9.81, 0, 0, 0, 0]]).T * 10.0 g = np.zeros((robot_config.N_JOINTS, 1)) for ii in range(robot_config.N_LINKS): pars = tuple(feedback["q"]) + tuple([0, 0, 0]) g += np.dot(J_links[ii](*pars).T, fake_gravity) u += g.squeeze()
np.random.random() * 2 + 1, 0]) # update the position of the target interface.set_target(target_xyz) path_planner.generate_path( position=hand_xyz, target_pos=target_xyz, plot=False) # returns desired [position, velocity] target, _ = path_planner.next() # generate an operational space control signal u = ctrlr.generate( q=feedback['q'], dq=feedback['dq'], target=np.hstack([target, np.zeros(3)]), ) # apply the control signal, step the sim forward interface.send_forces( u, update_display=True if count % 20 == 0 else False) count += 1 finally: # stop and reset the simulation interface.disconnect() dx_track = np.array(dx_track) * path_planner.dt plt.plot(dx_track, lw=2, label='End-effector velocity')