def test_admittance_ctrl(device, ctrl_manager, traj_gen, estimator_ft, adm_ctrl, dt=0.001): if(USE_ROBOT_VIEWER): viewer=robotviewer.client('XML-RPC'); N = 10*1000; axis = 1; fDes = np.zeros((N,6)); f = np.zeros((N,6)); ctrl_manager.setCtrlMode('all', 'adm'); traj_gen.startForceSinusoid('lf',axis, 100, 1.5); #name, axis, final force, time adm_ctrl.fLeftFoot.value = 6*(0,); adm_ctrl.fRightFoot.value = 6*(0,); for i in range(N): if(i==1500): traj_gen.stopForce('lf'); traj_gen.startForceSinusoid('lf',axis, -100, 1.5); #name, axis, final force, time device.increment (dt); sleep(dt); fDes[i,:] = traj_gen.fLeftFoot.value; f[i,:] = estimator_ft.contactWrenchLeftFoot.value; if(USE_ROBOT_VIEWER and i%30==0): viewer.updateElementConfig('hrp_device', list(device.state.value)+[0.0,]*10); if(i%100==0): # print "f(%.3f) = %.3f, \tfDes = %.3f" % (device.robotState.time*dt,f[i,axis],fDes[i,axis]); print ("fLeftFootError", adm_ctrl.fLeftFootError.value); print ("fRightFootError", adm_ctrl.fRightFootError.value); print ("dqDes", adm_ctrl.dqDes.value); (fig,ax) = create_empty_figure(3,1); ax[0].plot(fDes[:,0],'r'); ax[0].plot(f[:,0],'b'); ax[1].plot(fDes[:,1],'r'); ax[1].plot(f[:,1],'b'); ax[2].plot(fDes[:,2],'r'); ax[2].plot(f[:,2],'b'); plt.show();
def test_min_jerk(device, traj_gen): print "\nGonna move joint to -1.5..."; tt = 1.5; jid = 2; traj_gen.moveJoint('rhp', -1.5, tt); N = int(tt/dt); qMinJerk = np.zeros(N+2); dqMinJerk = np.zeros(N+2); ddqMinJerk = np.zeros(N+2); q = np.zeros(N+2); dq = np.zeros(N+2); if(USE_ROBOT_VIEWER): viewer=robotviewer.client('XML-RPC'); for i in range(N+2): device.increment (dt); qMinJerk[i] = traj_gen.q.value[jid]; dqMinJerk[i] = traj_gen.dq.value[jid]; ddqMinJerk[i] = traj_gen.ddq.value[jid]; q[i] = device.robotState.value[6+jid]; dq[i] = device.jointsVelocities.value[jid]; if(USE_ROBOT_VIEWER and i%30==0): viewer.updateElementConfig('hrp_device', list(device.state.value)+[0.0,]*10); if(i%100==0): print "q(%.3f) = %.3f, \tqDes = %.3f" % (device.robotState.time*dt,device.robotState.value[6+jid],traj_gen.q.value[jid]); dq_fd = np.diff(qMinJerk)/dt; ddq_fd = np.diff(dqMinJerk)/dt; (fig,ax) = create_empty_figure(3,1); ax[0].plot(qMinJerk,'r'); ax[0].plot(q,'b'); ax[1].plot(dqMinJerk,'r'); ax[1].plot(dq,'b'); ax[1].plot(dq_fd,'g--'); ax[2].plot(ddqMinJerk,'r'); ax[2].plot(ddq_fd,'g--'); plt.show();
def compute_estimates_from_sensors(sensors, delay, ftSensorOffsets=None, USE_FT_SENSORS=True): NJ = 30; # number of joints m = sensors['time'].shape[0]; # number of time steps dt = float(np.mean(sensors['time'][1:]-sensors['time'][:-1])); # sampling period # Create and initialize estimator estimator = ForceTorqueEstimator("estimator"+str(np.random.rand())); estimator.dqRef.value = NJ*(0.0,); estimator.ddqRef.value = NJ*(0.0,); #Only use inertia model (not current) to estimate torques. estimator.wCurrentTrust.value = NJ*(0.0,); estimator.currentMeasure.value = NJ*(0.0,); estimator.currentMeasure.value = NJ*(0.0,); estimator.saturationCurrent.value = NJ*(0.0,); estimator.motorParameterKt_p.value = tuple(30*[1.,]) estimator.motorParameterKt_n.value = tuple(30*[1.,]) estimator.motorParameterKf_p.value = tuple(30*[0.,]) estimator.motorParameterKf_n.value = tuple(30*[0.,]) estimator.motorParameterKv_p.value = tuple(30*[0.,]) estimator.motorParameterKv_n.value = tuple(30*[0.,]) estimator.motorParameterKa_p.value = tuple(30*[0.,]) estimator.motorParameterKa_n.value = tuple(30*[0.,]) set_sensor_data_in_estimator(estimator, sensors[0]); print "Time step: %f" % dt; print "Estimation delay: %f" % delay; if(ftSensorOffsets==None): estimator.init(dt,delay,delay,delay,delay,delay,True); else: estimator.init(dt,delay,delay,delay,delay,delay,False); estimator.setFTsensorOffsets(tuple(ftSensorOffsets)); estimator.setUseRawEncoders(False); estimator.setUseRefJointVel(False); estimator.setUseRefJointAcc(False); estimator.setUseFTsensors(USE_FT_SENSORS); torques = np.zeros((m,NJ)); dq = np.zeros((m,NJ)); ddq = np.zeros((m,NJ)); if(USE_ROBOT_VIEWER): viewer=robotviewer.client('XML-RPC'); for i in range(m): if(USE_ROBOT_VIEWER and i%10==0): viewer.updateElementConfig('hrp_device', [0,0,0.7,0,0,0] + sensors['enc'][i,:].tolist()); set_sensor_data_in_estimator(estimator, sensors[i]); estimator.jointsTorques.recompute(i); torques[i,:] = np.array(estimator.jointsTorques.value); dq[i,:] = np.array(estimator.jointsVelocities.value); ddq[i,:] = np.array(estimator.jointsAccelerations.value); if(i==2): print ("F/T sensor offsets are: ", estimator.getFTsensorOffsets()); if(i%1000==0): print 'Estimation time: \t %.3f' % (i*dt); return (torques, dq, ddq);
def initRobotViewer(): """Initialize robotviewer is possible.""" clt = None if hasRobotViewer: try: clt = robotviewer.client() except: print "failed to connect to robotviewer" return clt
def simulate(device, duration, dt=0.001): N = int(duration/dt); if(USE_ROBOT_VIEWER): viewer=robotviewer.client('XML-RPC'); for i in range(1,N): device.increment (dt); sleep(dt); if(USE_ROBOT_VIEWER): viewer.updateElementConfig('hrp_device', list(device.state.value)+[0.0,]*10);
def test_force_estimator(device, estimator_ft, estimator_kin, dt=0.001, estimationDelay=5): N = 1000 # test duration (in number of timesteps) freq = 0.5 # frequency A = 100 # amplitude f = np.zeros((4, 6)) f_est = np.zeros((4, 6)) err_f = np.zeros((4, 6)) count = 0 t = 0 if (USE_ROBOT_VIEWER): viewer = robotviewer.client('XML-RPC') print "Start simulation..." for i in range(1, N): t += dt axis = int(random.random() * 6) for j in range(4): f[j, axis] += A * (np.sin(2 * 3.14 * t) - np.sin(2 * 3.14 * (t - dt))) device.inputForceLARM.value = tuple(f[0, :]) device.inputForceRARM.value = tuple(f[1, :]) device.inputForceLLEG.value = tuple(f[2, :]) device.inputForceRLEG.value = tuple(f[3, :]) device.increment(dt) if (USE_ROBOT_VIEWER): viewer.updateElementConfig('hrp_device', list(device.state.value) + [ 0.0, ] * 10) if (i % estimationDelay == 0): f_est[0, :] = np.array(estimator_ft.contactWrenchLeftHand.value) f_est[1, :] = np.array(estimator_ft.contactWrenchRightHand.value) f_est[2, :] = np.array(estimator_ft.contactWrenchLeftFoot.value) f_est[3, :] = np.array(estimator_ft.contactWrenchRightFoot.value) if (i > estimationDelay): print("Time", i, "f est", f_est[0, :], "f real", f[0, :]) err_f += abs(f_est - f) count += 1 print "*************************************************************************************************\n" print "Amplitude of force variation: %.2f" % (A) print "Frequency of force variation: %.2f" % (freq) print "Test duration: %.3f s" % (N * dt) print "Time step: %.3f s" % dt print "Estimation delay: %.3f" % (estimationDelay * dt) if (count > 0): print "Average percentual errors:" print(f_est / count)
def test_chirp(device, traj_gen, estimator_ft): device.after.addDownsampledSignal('estimator_ft.ftSensRightFootPrediction', 1) print "Start linear chirp from %f to -1.0" % device.robotState.value[6 + 2] tt = 4 jid = 1 traj_gen.startLinChirp('rhr', -0.6, 0.2, 2.0, tt) N = int(tt / dt) qChirp = np.zeros(N + 2) dqChirp = np.zeros(N + 2) ddqChirp = np.zeros(N + 2) q = np.zeros(N + 2) dq = np.zeros(N + 2) if (USE_ROBOT_VIEWER): viewer = robotviewer.client('XML-RPC') viewer.updateElementConfig('hrp_device', list(device.state.value) + [ 0.0, ] * 10) for i in range(N + 2): device.increment(dt) qChirp[i] = traj_gen.q.value[jid] dqChirp[i] = traj_gen.dq.value[jid] ddqChirp[i] = traj_gen.ddq.value[jid] q[i] = device.robotState.value[6 + jid] dq[i] = device.jointsVelocities.value[jid] # print "q(%.3f) = %.3f, \tqDes = %.3f" % (device.robotState.time*dt,device.robotState.value[6+jid],traj_gen.q.value[jid]); # print "dq(%.3f) = %.3f, \tdqDes = %.3f" % (device.robotState.time*dt,device.jointsVelocities.value[6+jid],traj_gen.dq.value[jid]); # print "tauDes(%.3f)= %.3f" % (device.robotState.time*dt,inv_dyn.tauDes.value[jid]); # sleep(0.1); if (USE_ROBOT_VIEWER and i % 30 == 0): viewer.updateElementConfig('hrp_device', list(device.state.value) + [ 0.0, ] * 10) if (i % 100 == 0): print("FT sensor right foot prediction: ", estimator_ft.ftSensRightFootPrediction.value) # print "q(%.3f) = %.3f, \tqDes = %.3f" % (device.robotState.time*dt,device.robotState.value[6+jid],traj_gen.q.value[jid]); dq_fd = np.diff(qChirp) / dt ddq_fd = np.diff(dqChirp) / dt (fig, ax) = create_empty_figure(3, 1) ax[0].plot(qChirp, 'r') ax[0].plot(q, 'b') ax[1].plot(dqChirp, 'r') ax[1].plot(dq, 'b') ax[1].plot(dq_fd, 'g--') ax[2].plot(ddqChirp, 'r') ax[2].plot(ddq_fd, 'g--') plt.show()
def test_vel_acc_estimator(device, estimator_ft, estimator_kin, dt=0.001, estimationDelay = 5): j = 0; # index of joint under analysis N = 1000; # test duration (in number of timesteps) q = device.state.value; rad2deg = 180/3.14; q_real = rad2deg*q[j+6]; dq_real = 0; ddq_real = 0; err_q = 0; err_dq = 0; err_ddq = 0; count = 0; t = estimator_ft.jointsTorques.time; if(USE_ROBOT_VIEWER): viewer=robotviewer.client('XML-RPC'); print "Start simulation..." for i in range(1,N): device.increment (dt); if(USE_ROBOT_VIEWER): viewer.updateElementConfig('hrp_device', list(device.state.value)+[0.0,]*10); estimator_ft.jointsTorques.recompute (t+i); if(i%estimationDelay == 0): q_real = rad2deg*device.robotState.value[j+6]; dq_real = rad2deg*device.jointsVelocities.value[j]; ddq_real = rad2deg*device.jointsAccelerations.value[j]; if(i>estimationDelay): print "\nTime %.3f) q est VS real = (%.2f, %.2f), dq est VS real (%.2f, %.2f) ddq est VS real (%.2f, %.2f)" % (i*dt, \ rad2deg*estimator_kin.x_filtered.value[j], q_real, \ rad2deg*estimator_kin.dx.value[j], dq_real, \ rad2deg*estimator_kin.ddx.value[j], ddq_real); print "Torque %f" % (estimator_ft.jointsTorques.value[j]); if(abs(ddq_real)<1e-2): ddq_real = 1e-2; if(q_real!=0.0): err_q += abs((rad2deg*estimator_kin.x_filtered.value[j] - q_real)/q_real); if(dq_real!=0.0): err_dq += abs((rad2deg*estimator_kin.dx.value[j] - dq_real)/dq_real); if(ddq_real!=0.0): err_ddq += abs((rad2deg*estimator_kin.ddx.value[j] - ddq_real)/ddq_real); count += 1; print "*************************************************************************************************\n"; print "Testing joint %d" % j; print "Initial position of joint %d: %.2f" % (j, rad2deg*q[6+j]); print "Desired position of joint %d: %.2f" % (j, rad2deg*device.control.value[j]); print "Test duration: %.3f s" % (N*dt); print "Time step: %.3f s" % dt; print "Estimation delay: %.3f" % (estimationDelay*dt); if(count>0): print "Average percentual errors:\n* position: %.2f %%\n* velocity: %.2f %%\n* acceleration: %.2f %%" % \ (100*err_q/count, 100*err_dq/count, 100*err_ddq/count);
def test_force_jacobians(device, estimator_ft, torque_ctrl, traj_gen, ctrl_manager, inv_dyn, dt): if (USE_ROBOT_VIEWER): viewer = robotviewer.client('XML-RPC') ctrl_manager.setCtrlMode("rhr", "torque") ctrl_manager.setCtrlMode("rhp", "torque") ctrl_manager.setCtrlMode("rhy", "torque") ctrl_manager.setCtrlMode("rk", "torque") ctrl_manager.setCtrlMode("rap", "torque") ctrl_manager.setCtrlMode("rar", "torque") inv_dyn.Kp.value = NJ * (0.0, ) inv_dyn.Kd.value = NJ * (0.0, ) inv_dyn.Kf.value = (6 * 4) * (1.0, ) N = 1 #10*1000; axis = 2 FORCE_MAX = 0.1 tauFB1 = np.zeros((N, 30)) tauFB2 = np.zeros((N, 30)) traj_gen.startForceSinusoid('rf', axis, FORCE_MAX, 1.5) #name, axis, final force, time for i in range(N): if (i == 1500): traj_gen.stopForce('rf') traj_gen.startForceSinusoid('rf', axis, -FORCE_MAX, 1.5) #name, axis, final force, time device.increment(dt) sleep(dt) if (USE_ROBOT_VIEWER and i % 100 == 0): viewer.updateElementConfig('hrp_device', list(device.state.value) + [ 0.0, ] * 10) inv_dyn.tauFB2.recompute(i) tauFB1[i, :] = inv_dyn.tauFB.value tauFB2[i, :] = inv_dyn.tauFB2.value (fig, ax) = create_empty_figure(3, 1) ax[0].plot(tauFB1[:, 0], 'r') ax[0].plot(tauFB2[:, 0], 'b--') ax[1].plot(tauFB1[:, 1], 'r') ax[1].plot(tauFB2[:, 1], 'b--') ax[2].plot(tauFB1[:, 2], 'r') ax[2].plot(tauFB2[:, 2], 'b--') (fig, ax) = create_empty_figure(3, 1) ax[0].plot(tauFB1[:, 3], 'r') ax[0].plot(tauFB2[:, 3], 'b--') ax[1].plot(tauFB1[:, 4], 'r') ax[1].plot(tauFB2[:, 4], 'b--') ax[2].plot(tauFB1[:, 5], 'r') ax[2].plot(tauFB2[:, 5], 'b--') plt.show()
def addRobotViewer(robot, **args): ''' Arguments are: * small=False * server='XML-RPC' == { 'XML-RPC' | 'CORBA' } * verbose=True ''' verbose = args.get('verbose', True) small = args.get('small', False) small_extra = args.get('small_extra', 10) server = args.get('server', 'XML-RPC') try: import robotviewer RobotClass = robot.__class__ if small: if verbose: print('using a small robot') RobotClass.stateFullSize = lambda x: \ stateFullSize(x, small_extra * (0.0, )) else: RobotClass.stateFullSize = stateFullSize robot.viewer = robotviewer.client(server) RobotClass.refresh = refreshView RobotClass.incrementNoView = RobotClass.increment RobotClass.increment = incrementView RobotClass.setNoView = RobotClass.set RobotClass.set = setView robot.displayList = [] # Check the connection if args.get('dorefresh', True): robot.refresh() except Exception: if verbose: print("No robot viewer, sorry.") robot.viewer = RobotViewerFaked()
def test_min_jerk(device, traj_gen, dt=0.001): print("\nGonna move joint to -1.5...") tt = 1.5 jid = 2 traj_gen.moveJoint('rhp', -1.5, tt) N = int(tt / dt) qMinJerk = np.zeros(N + 2) dqMinJerk = np.zeros(N + 2) ddqMinJerk = np.zeros(N + 2) q = np.zeros(N + 2) dq = np.zeros(N + 2) if (USE_ROBOT_VIEWER): viewer = robotviewer.client('XML-RPC') for i in range(N + 2): device.increment(dt) qMinJerk[i] = traj_gen.q.value[jid] dqMinJerk[i] = traj_gen.dq.value[jid] ddqMinJerk[i] = traj_gen.ddq.value[jid] q[i] = device.robotState.value[6 + jid] dq[i] = device.jointsVelocities.value[jid] if (USE_ROBOT_VIEWER and i % 30 == 0): viewer.updateElementConfig('hrp_device', list(device.state.value) + [0.0] * 10) if i % 100 == 0: print("q(%.3f) = %.3f, \tqDes = %.3f" % (device.robotState.time * dt, device.robotState.value[6 + jid], traj_gen.q.value[jid])) dq_fd = np.diff(qMinJerk) / dt ddq_fd = np.diff(dqMinJerk) / dt (fig, ax) = create_empty_figure(3, 1) ax[0].plot(qMinJerk, 'r') ax[0].plot(q, 'b') ax[1].plot(dqMinJerk, 'r') ax[1].plot(dq, 'b') ax[1].plot(dq_fd, 'g--') ax[2].plot(ddqMinJerk, 'r') ax[2].plot(ddq_fd, 'g--') plt.show()
def addRobotViewer(robot,**args): ''' Arguments are: * small=False * server='XML-RPC' == { 'XML-RPC' | 'CORBA' } * verbose=True ''' verbose = args.get('verbose',True) small = args.get('small',False) small_extra = args.get('small_extra',10) server = args.get('server','XML-RPC') try: import robotviewer RobotClass = robot.__class__ if small: if verbose: print 'using a small robot' RobotClass.stateFullSize = lambda x: stateFullSize(x,small_extra*(0.0,)) else: RobotClass.stateFullSize = stateFullSize robot.viewer = robotviewer.client(server) RobotClass.refresh = refreshView RobotClass.incrementNoView = RobotClass.increment RobotClass.increment = incrementView RobotClass.setNoView = RobotClass.set RobotClass.set = setView robot.displayList= [] # Check the connection if args.get('dorefresh',True): robot.refresh() except: if verbose: print "No robot viewer, sorry." robot.viewer = RobotViewerFaked()
#------------------------------------------------------ # Try to read the input file, if possible try: fileName = prefix+'position.dat' file_dyn_position = open(fileName,'r') # Open the file except: print "Non valid input file ",fileName sys.exit(0) #------------------------------------------------------ # Initialize robotviewer, if possible try: import robotviewer clt = robotviewer.client(options.server) except: print "No robot viewer, sorry." sys.exit(0) #----------------------------------------------------- nbImage=0 lastImage=0 robotTime = 2 hq=[] def inc(): global robotTime, q, hq, robotSize, nbImage, lastImage, decimate
from dynamic_graph.sot.hrp2_14.robot import Robot from dynamic_graph.sot.dynamics.test import Stepper from dynamic_graph import enableTrace, plug from dynamic_graph.sot.dynamics.solver import Solver rvName = "hrp" def toViewerConfig(config): return config + 10 * (0.0,) try: import robotviewer clt = robotviewer.client() hasRobotViewer = True except: hasRobotViewer = False print "no robotviewer" timeStep = 0.005 gravity = 9.81 class Motion(object): signalList = [] def __init__(self, robot, solver): self.solver = solver self.robot = robot
default="CORBA", ) (options, args) = parser.parse_args() parser.print_help() print "\n -------------------------------------------------------------------------" # --- DISPLAY INIT -------------------------------------------------------------------- # --- DISPLAY INIT -------------------------------------------------------------------- # --- DISPLAY INIT -------------------------------------------------------------------- # Initialize robotviewer, if possible try: import robotviewer clt = robotviewer.client(options.server) # default is CORBA except: print " -- WARNING!: No robot viewer (or not correct server)." class RobotViewerFaked: def update(*args): void def updateElementConfig(*args): void clt = RobotViewerFaked() # --- PARSER INIT ------------------------------------------------------------------ # --- PARSER INIT ------------------------------------------------------------------
''' Execute several trajectory interpolation for various se(3) velocities. The purpose of this script is to exhibit the use of the exp and log map for interpolating SE(3) movements. ''' import robotviewer viewer = robotviewer.client('XML-RPC') try: viewer.updateElementConfig('RomeoTrunkYaw', [1, 0, 0, 0, 0, 0]) except: viewer.updateElementConfig = lambda a, b: a def se3ToRpy(m): return M.translation.T.tolist()[0] + matrixToRpy(M.rotation).T.tolist()[0] import pinocchio as se3 from pinocchio.utils import * norm = npl.norm import time N = 1000 M = se3.SE3.Identity() # Integrate a constant body velocity. v = zero(3) v[2] = 1.0 / N w = zero(3) w[1] = 1.0 / N
# half sitting # qhs=matrix((0,0,0,0,0,0, 0,0,-26,50,-24,0,0,0,-26,50,-24,0,0,0,0,0,15,10,0,-30,0,0,10,15,-10,0,-30,0,0,10)) # robot.set((0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.4537856055185257, 0.87266462599716477, -0.41887902047863906, 0.0, 0.0, 0.0, -0.4537856055185257, 0.87266462599716477, -0.41887902047863906, 0.0, 0.0, 0.0, 0.0, 0.0, 0.26179938779914941, 0.17453292519943295, 0.0, -0.52359877559829882, 0.0, 0.0, 0.17453292519943295, 0.26179938779914941, -0.17453292519943295, 0.0, -0.52359877559829882, 0.0, 0.0, 0.17453292519943295)) # ------------------------------------------------------------------------------ # --- VIEWER ------------------------------------------------------------------- # ------------------------------------------------------------------------------ try: import robotviewer def stateFullSize(robot): return [float(val) for val in robot.state.value] + 10 * [0.0] RobotClass.stateFullSize = stateFullSize robot.viewer = robotviewer.client("XML-RPC") # robot.viewer = robotviewer.client('CORBA') # Check the connection robot.viewer.updateElementConfig("hrp", robot.stateFullSize()) def refreshView(robot): robot.viewer.updateElementConfig("hrp", robot.stateFullSize()) RobotClass.refresh = refreshView def incrementView(robot, dt): robot.incrementNoView(dt) robot.refresh() # if zmp.zmp.time > 0: # robot.viewer.updateElementConfig('zmp',[zmp.zmp.value[0],zmp.zmp.value[1],0,0,0,0])
def compute_estimates_from_sensors(sensors, delay, ftSensorOffsets=None, USE_FT_SENSORS=True): NJ = 30; # number of joints m = sensors['time'].shape[0]; # number of time steps dt = float(np.mean(sensors['time'][1:]-sensors['time'][:-1])); # sampling period # Create and initialize estimator estimator_ft = ForceTorqueEstimator("estimator_ft"+str(np.random.rand())); estimator_kin = VelAccEstimator("estimator_kin"+str(np.random.rand())); plug(estimator_kin.x, estimator_ft.q_filtered); plug(estimator_kin.dx, estimator_ft.dq_filtered); plug(estimator_kin.ddx, estimator_ft.ddq_filtered); estimator_ft.dqRef.value = NJ*(0.0,); estimator_ft.ddqRef.value = NJ*(0.0,); #Only use inertia model (not current) to estimate torques. estimator_ft.wCurrentTrust.value = NJ*(0.0,); estimator_ft.currentMeasure.value = NJ*(0.0,); estimator_ft.saturationCurrent.value = NJ*(0.0,); estimator_ft.motorParameterKt_p.value = tuple(30*[1.,]) estimator_ft.motorParameterKt_n.value = tuple(30*[1.,]) estimator_ft.motorParameterKf_p.value = tuple(30*[0.,]) estimator_ft.motorParameterKf_n.value = tuple(30*[0.,]) estimator_ft.motorParameterKv_p.value = tuple(30*[0.,]) estimator_ft.motorParameterKv_n.value = tuple(30*[0.,]) estimator_ft.motorParameterKa_p.value = tuple(30*[0.,]) estimator_ft.motorParameterKa_n.value = tuple(30*[0.,]) set_sensor_data_in_estimator(estimator_ft, estimator_kin, sensors[0]); print "Time step: %f" % dt; print "Estimation delay: %f" % delay; if(ftSensorOffsets==None): estimator_ft.init(dt,delay,delay,delay,delay,True); else: estimator_ft.init(dt,delay,delay,delay,delay,False); estimator_ft.setFTsensorOffsets(tuple(ftSensorOffsets)); estimator_kin.init(dt,NJ, delay); estimator_ft.setUseRawEncoders(False); estimator_ft.setUseRefJointVel(False); estimator_ft.setUseRefJointAcc(False); estimator_ft.setUseFTsensors(USE_FT_SENSORS); torques = np.zeros((m,NJ)); dq = np.zeros((m,NJ)); ddq = np.zeros((m,NJ)); if(USE_ROBOT_VIEWER): viewer=robotviewer.client('XML-RPC'); for i in range(m): if(USE_ROBOT_VIEWER and i%10==0): viewer.updateElementConfig('hrp_device', [0,0,0.7,0,0,0] + sensors['enc'][i,:].tolist()); set_sensor_data_in_estimator(estimator_ft, estimator_kin, sensors[i]); estimator_ft.jointsTorques.recompute(i); torques[i,:] = np.array(estimator_ft.jointsTorques.value); dq[i,:] = np.array(estimator_kin.dx.value); ddq[i,:] = np.array(estimator_kin.ddx.value); if(i==2): print ("F/T sensor offsets are: ", estimator_ft.getFTsensorOffsets()); if(i%1000==0): print 'Estimation time: \t %.3f' % (i*dt); return (torques, dq, ddq);
def compute_estimates_from_sensors(sensors, delay, ftSensorOffsets=None, USE_FT_SENSORS=True): NJ = 30 # number of joints m = sensors['time'].shape[0] # number of time steps dt = float(np.mean(sensors['time'][1:] - sensors['time'][:-1])) # sampling period # Create and initialize estimator estimator_ft = ForceTorqueEstimator("estimator_ft" + str(np.random.rand())) estimator_kin = NumericalDifference("estimator_kin" + str(np.random.rand())) ft_RF_filter = NumericalDifference("ft_RF_filter" + str(np.random.rand())) ft_LF_filter = NumericalDifference("ft_LF_filter" + str(np.random.rand())) ft_RH_filter = NumericalDifference("ft_RH_filter" + str(np.random.rand())) ft_LH_filter = NumericalDifference("ft_LH_filter" + str(np.random.rand())) acc_filter = NumericalDifference("dv_filter" + str(np.random.rand())) gyro_filter = NumericalDifference("w_filter" + str(np.random.rand())) estimator_kin = NumericalDifference("estimator_kin" + str(np.random.rand())) plug(estimator_kin.x, estimator_ft.q_filtered) plug(estimator_kin.dx, estimator_ft.dq_filtered) plug(estimator_kin.ddx, estimator_ft.ddq_filtered) plug(acc_filter.x_filtered, estimator_ft.accelerometer) plug(gyro_filter.x_filtered, estimator_ft.gyro) plug(gyro_filter.dx, estimator_ft.dgyro) plug(ft_RF_filter.x_filtered, estimator_ft.ftSensRightFoot) plug(ft_LF_filter.x_filtered, estimator_ft.ftSensLeftFoot) plug(ft_RH_filter.x_filtered, estimator_ft.ftSensRightHand) plug(ft_LH_filter.x_filtered, estimator_ft.ftSensLeftHand) estimator_ft.rotor_inertias.value = NJ * (0., ) estimator_ft.gear_ratios.value = NJ * (0., ) estimator_ft.dqRef.value = NJ * (0.0, ) estimator_ft.ddqRef.value = NJ * (0.0, ) # Only use inertia model (not current) to estimate torques. estimator_ft.wCurrentTrust.value = NJ * (0.0, ) estimator_ft.current.value = NJ * (0.0, ) estimator_ft.saturationCurrent.value = NJ * (0.0, ) estimator_ft.motorParameterKt_p.value = tuple(30 * [1.]) estimator_ft.motorParameterKt_n.value = tuple(30 * [1.]) estimator_ft.motorParameterKf_p.value = tuple(30 * [0.]) estimator_ft.motorParameterKf_n.value = tuple(30 * [0.]) estimator_ft.motorParameterKv_p.value = tuple(30 * [0.]) estimator_ft.motorParameterKv_n.value = tuple(30 * [0.]) estimator_ft.motorParameterKa_p.value = tuple(30 * [0.]) estimator_ft.motorParameterKa_n.value = tuple(30 * [0.]) set_sensor_data_in_estimator(estimator_ft, estimator_kin, acc_filter, gyro_filter, ft_RH_filter, ft_LF_filter, ft_LH_filter, ft_RF_filter, sensors[0]) print("Time step: %f" % dt) print("Estimation delay: %f" % delay) if ftSensorOffsets is None: estimator_ft.init(True) else: estimator_ft.init(False) estimator_ft.setFTsensorOffsets(tuple(ftSensorOffsets)) estimator_kin.init(dt, NJ, delay, 2) ft_RF_filter.init(dt, 6, delay, 1) ft_LF_filter.init(dt, 6, delay, 1) ft_RH_filter.init(dt, 6, delay, 1) ft_LH_filter.init(dt, 6, delay, 1) gyro_filter.init(dt, 3, delay, 1) acc_filter.init(dt, 3, delay, 1) estimator_ft.setUseRawEncoders(False) estimator_ft.setUseRefJointVel(False) estimator_ft.setUseRefJointAcc(False) estimator_ft.setUseFTsensors(USE_FT_SENSORS) torques = np.zeros((m, NJ)) dq = np.zeros((m, NJ)) ddq = np.zeros((m, NJ)) if (USE_ROBOT_VIEWER): viewer = robotviewer.client('XML-RPC') for i in range(m): if USE_ROBOT_VIEWER and i % 10 == 0: viewer.updateElementConfig('hrp_device', [0, 0, 0.7, 0, 0, 0] + sensors['enc'][i, :].tolist()) set_sensor_data_in_estimator(estimator_ft, estimator_kin, acc_filter, gyro_filter, ft_RH_filter, ft_LF_filter, ft_LH_filter, ft_RF_filter, sensors[i]) estimator_ft.jointsTorques.recompute(i) torques[i, :] = np.array(estimator_ft.jointsTorques.value) dq[i, :] = np.array(estimator_kin.dx.value) ddq[i, :] = np.array(estimator_kin.ddx.value) if i == 2: print("F/T sensor offsets are: ", estimator_ft.getFTsensorOffsets()) if i % 1000 == 0: print('Estimation time: \t %.3f' % (i * dt)) return (torques, dq, ddq)
parser.add_option("-d", dest="delayTime", help="Delay time between iterations (in sec). Default is %default", \ type=float, default="0") parser.add_option("-s", dest="server", help="Name of server (CORBA or XML-RPC). Default is %default", \ metavar="NAME_SERVER", default="CORBA") (options, args) = parser.parse_args() parser.print_help() print '\n -------------------------------------------------------------------------' # --- DISPLAY INIT -------------------------------------------------------------------- # --- DISPLAY INIT -------------------------------------------------------------------- # --- DISPLAY INIT -------------------------------------------------------------------- # Initialize robotviewer, if possible try: import robotviewer clt = robotviewer.client(options.server) # default is CORBA except: print " -- WARNING!: No robot viewer (or not correct server)." class RobotViewerFaked: def update(*args): void def updateElementConfig(*args): void clt = RobotViewerFaked() # --- PARSER INIT ------------------------------------------------------------------ # --- PARSER INIT ------------------------------------------------------------------ # --- PARSER INIT ------------------------------------------------------------------
robot = RobotClass("robot") robot.resize(robotDim) robot.set( initialConfig[robotName] ) dt=5e-3 # --- VIEWER ------------------------------------------------------------------- # --- VIEWER ------------------------------------------------------------------- # --- VIEWER ------------------------------------------------------------------- try: import robotviewer def stateFullSize(robot): return [float(val) for val in robot.state.value]+10*[0.0] RobotClass.stateFullSize = stateFullSize robot.viewer = robotviewer.client('XML-RPC') # Check the connection robot.viewer.updateElementConfig('hrp',robot.stateFullSize()) def refreshView( robot ): robot.viewer.updateElementConfig('hrp',robot.stateFullSize()) RobotClass.refresh = refreshView def incrementView( robot,dt ): robot.incrementNoView(dt) robot.refresh() RobotClass.incrementNoView = RobotClass.increment RobotClass.increment = incrementView def setView( robot,*args ): robot.setNoView(*args) robot.refresh() RobotClass.setNoView = RobotClass.set
else: delayTime = float(options.delayTime) #------------------------------------------------------ # Try to read the input file, if possible try: fileName = prefix + 'position.dat' file_dyn_position = open(fileName, 'r') # Open the file except: print "Non valid input file ", fileName sys.exit(0) #------------------------------------------------------ # Initialize robotviewer, if possible try: import robotviewer clt = robotviewer.client(options.server) except: print "No robot viewer, sorry." sys.exit(0) #----------------------------------------------------- nbImage = 0 lastImage = 0 robotTime = 2 hq = [] def inc(): global robotTime, q, hq, robotSize, nbImage, lastImage, decimate while True:
#fileName = sys.argv[1] # Input File ([2] if using 'py', [1] if 'python') prefix='mocap/' fileName = prefix+'yoganmsd.pos' file_dyn_position = open(fileName,'r') # Open the file # fileffName = prefix+'ffposition.dat' # file_dyn_ffposition = open(fileffName,'r') # Open the file except: print "Please, specify the input file" sys.exit(0) #------------------------------------------------------ # Initialize robotviewer, if possible try: import robotviewer clt = robotviewer.client('XML-RPC') except: print "No robot viewer, sorry." sys.exit(0) #----------------------------------------------------- # Main Loop robotTime = 1 hq=[] def inc(): global robotTime, q, hq while True: pos_angles = file_dyn_position.readline() if len(pos_angles)==0: break