def example_script(name_interface): slider_box = SliderBox() device = Solo12(name_interface,dt=0.001) nb_motors = device.nb_motors device.init(calibrateEncoders=True) #CONTROL LOOP *************************************************** while ((not device.hardware.IsTimeout()) and (clock() < 200) and not slider_box.get_e_stop()): device.UpdateMeasurment() device.SetDesiredJointTorque([0]*nb_motors) device.SendCommand(WaitEndOfCycle=True) if ((device.cpt % 100) == 0): device.Print() #**************************************************************** # Whatever happened we send 0 torques to the motors. device.SetDesiredJointTorque([0]*nb_motors) device.SendCommand(WaitEndOfCycle=True) if device.hardware.IsTimeout(): print("Masterboard timeout detected.") print("Either the masterboard has been shut down or there has been a connection issue with the cable/wifi.") device.hardware.Stop() # Shut down the interface between the computer and the master board
def example_script(name_interface): # making code compatible with python 2 and 3 if hasattr(builtins, "raw_input"): input = builtins.raw_input else: input = builtins.input global init_done, calibration_done # setting up the callback on space keystroke keyboard.on_press_key('space', return_callback) # Motors not being calibrated will be held at position 0 q_ref = 0 v_ref = 0 kp = 0.125 # Proportional gain kd = 0.0025 # Derivative gain # Change this with your robot device = Solo12(name_interface, dt=0.001) nb_motors = device.nb_motors print("") print( "Enter a binary number that represents the motors you want to calibrate." ) print("LSB: first motor, MSB: last motor") print( "Example with 8 motors: input \"10011010\" will calibrate motor 1, 3, 4, 7." ) print("") try: bin_motors = int(input(), 2) # getting binary representation of motors to be calibrated from user print("") except: print("") raise ValueError("Wrong input received, exiting.") if bin_motors == 0: raise ValueError("0 received, no calibration to be done, exiting.") elif bin_motors < 0 or bin_motors >= (1 << nb_motors): raise ValueError( "An invalid number for your configuration has been received, no calibration to be done, exiting." ) not_to_calibrate = [] for i in range(nb_motors): if ((bin_motors & (1 << device.motorToUrdf[i])) >> device.motorToUrdf[i]): # if motor needs to be calibrated device.encoderOffsets[i] = 0 # set offset to zero else: not_to_calibrate.append(device.motorToUrdf[i]) device.Init( calibrateEncoders=True) # finding encoder index happens in here init_done = True if not device.hardware.IsTimeout(): print("") print("Index detected!") print("Please move the motors to calibrate to the desired zero.") print("Press the SPACE key when done...") # --- Control loop --- while ((not device.hardware.IsTimeout()) and not calibration_done): device.UpdateMeasurment() torques = [0] * nb_motors for i in not_to_calibrate: q_err = q_ref - device.q_mes[i] # Position error v_err = v_ref - device.v_mes[i] # Velocity error torques[i] = kp * q_err + kd * v_err device.SetDesiredJointTorque(torques) device.SendCommand(WaitEndOfCycle=True) # --- if calibration_done: device.SetDesiredJointTorque([0] * nb_motors) device.SendCommand(WaitEndOfCycle=True) print("") print("Calibration done.") print( "Please paste the following value into your RobotHAL file after \"self.encoderOffsets = \":" ) print("") print("- np.array({})".format([(device.hardware.GetMotor(i).GetPosition() if (bin_motors & (1 << device.motorToUrdf[i])) >> device.motorToUrdf[i] else - device.encoderOffsets[i]) \ for i in range(nb_motors)])) elif device.hardware.IsTimeout(): print("") print("Interface timed out, rerun calibration.") print("") device.hardware.Stop( ) # Shut down the interface between the computer and the master board
def mcapi_playback(name_interface): """Main function that calibrates the robot, get it into a default waiting position then launch the main control loop once the user has pressed the Enter key Args: name_interface (string): name of the interface that is used to communicate with the robot """ ######################################### # PARAMETERS OF THE MPC-TSID CONTROLLER # ######################################### envID = 0 # Identifier of the environment to choose in which one the simulation will happen velID = 0 # Identifier of the reference velocity profile to choose which one will be sent to the robot dt_tsid = 0.0020 # Time step of TSID dt_mpc = 0.02 # Time step of the MPC k_mpc = int( dt_mpc / dt_tsid) # dt is dt_tsid, defined in the TSID controller script t = 0.0 # Time n_periods = 1 # Number of periods in the prediction horizon T_gait = 0.64 # Duration of one gait period N_SIMULATION = 50000 # number of simulated TSID time steps # Which MPC solver you want to use # True to have PA's MPC, to False to have Thomas's MPC type_MPC = True # Whether PyBullet feedback is enabled or not pyb_feedback = False # Whether we are working with solo8 or not on_solo8 = False # If True the ground is flat, otherwise it has bumps use_flat_plane = True # If we are using a predefined reference velocity (True) or a joystick (False) predefined_vel = False # Enable or disable PyBullet GUI enable_pyb_GUI = True # Default position after calibration q_init = np.array( [0.0, 0.8, -1.6, 0, 0.8, -1.6, 0, -0.8, 1.6, 0, -0.8, 1.6]) # Run a scenario and retrieve data thanks to the logger controller = Controller(q_init, envID, velID, dt_tsid, dt_mpc, k_mpc, t, n_periods, T_gait, N_SIMULATION, type_MPC, pyb_feedback, on_solo8, use_flat_plane, predefined_vel, enable_pyb_GUI) #### if SIMULATION: device = PyBulletSimulator() qc = None else: device = Solo12(name_interface, dt=DT) qc = QualisysClient(ip="140.93.16.160", body_id=0) if LOGGING: logger = Logger(device, qualisys=qc, logSize=N_SIMULATION) # Number of motors nb_motors = device.nb_motors q_viewer = np.array((7 + nb_motors) * [ 0., ]) # Initiate communication with the device and calibrate encoders if SIMULATION: device.Init(calibrateEncoders=True, q_init=q_init, envID=envID, use_flat_plane=use_flat_plane, enable_pyb_GUI=enable_pyb_GUI, dt=dt_tsid) else: device.Init(calibrateEncoders=True, q_init=q_init) # Wait for Enter input before starting the control loop put_on_the_floor(device, q_init) # CONTROL LOOP *************************************************** t = 0.0 t_max = (N_SIMULATION - 2) * dt_tsid while ((not device.hardware.IsTimeout()) and (t < t_max)): device.UpdateMeasurment() # Retrieve data from IMU and Motion capture # Desired torques tau = controller.compute(device) # print(tau[0:3].ravel()) tau = tau.ravel() # Set desired torques for the actuators device.SetDesiredJointTorque(tau) # Call logger if LOGGING: logger.sample(device, qualisys=qc) # Send command to the robot device.SendCommand(WaitEndOfCycle=True) if ((device.cpt % 1000) == 0): device.Print() t += DT # **************************************************************** # Stop MPC running in a parallel process if controller.enable_multiprocessing: print("Stopping parallel process") controller.mpc_wrapper.stop_parallel_loop() # Whatever happened we send 0 torques to the motors. device.SetDesiredJointTorque([0] * nb_motors) device.SendCommand(WaitEndOfCycle=True) if device.hardware.IsTimeout(): print("Masterboard timeout detected.") print( "Either the masterboard has been shut down or there has been a connection issue with the cable/wifi." ) device.hardware.Stop( ) # Shut down the interface between the computer and the master board # Save the logs of the Logger object if LOGGING: logger.saveAll() print("Log saved") if SIMULATION and enable_pyb_GUI: # Disconnect the PyBullet server (also close the GUI) device.Stop() print("End of script") quit()
def mcapi_playback(name_interface): """Main function that calibrates the robot, get it into a default waiting position then launch the main control loop once the user has pressed the Enter key Args: name_interface (string): name of the interface that is used to communicate with the robot """ if SIMULATION: device = PyBulletSimulator() qc = None else: device = Solo12(name_interface, dt=DT) qc = QualisysClient(ip="140.93.16.160", body_id=0) if LOGGING: logger = Logger(device, qualisys=qc) # Number of motors nb_motors = device.nb_motors q_viewer = np.array((7 + nb_motors) * [ 0., ]) # Gepetto-gui v = viewerClient() v.display(q_viewer) # PyBullet GUI enable_pyb_GUI = True # Maximum duration of the demonstration t_max = 300.0 # Default position after calibration q_init = np.array([0, 0.8, -1.6, 0, 0.8, -1.6, 0, -0.8, 1.6, 0, -0.8, 1.6]) # Create Estimator object estimator = Estimator(DT, np.int(t_max / DT)) # Set the paths where the urdf and srdf file of the robot are registered modelPath = "/opt/openrobots/share/example-robot-data/robots" urdf = modelPath + "/solo_description/robots/solo12.urdf" vector = pin.StdVec_StdString() vector.extend(item for item in modelPath) # Create the robot wrapper from the urdf model (which has no free flyer) and add a free flyer robot = tsid.RobotWrapper(urdf, vector, pin.JointModelFreeFlyer(), False) model = robot.model() # Creation of the Invverse Dynamics HQP problem using the robot # accelerations (base + joints) and the contact forces invdyn = tsid.InverseDynamicsFormulationAccForce("tsid", robot, False) # Compute the problem data with a solver based on EiQuadProg invdyn.computeProblemData(0.0, np.hstack((np.zeros(7), q_init)), np.zeros(18)) # Initiate communication with the device and calibrate encoders if SIMULATION: device.Init(calibrateEncoders=True, q_init=q_init, envID=0, use_flat_plane=True, enable_pyb_GUI=enable_pyb_GUI, dt=DT) else: device.Init(calibrateEncoders=True, q_init=q_init) # Wait for Enter input before starting the control loop put_on_the_floor(device, q_init) # CONTROL LOOP *************************************************** t = 0.0 k = 0 while ((not device.hardware.IsTimeout()) and (t < t_max)): device.UpdateMeasurment() # Retrieve data from IMU and Motion capture # Run estimator with hind left leg touching the ground estimator.run_filter(k, np.array([0, 0, 1, 0]), device, invdyn.data(), model) # Zero desired torques tau = np.zeros(12) # Set desired torques for the actuators device.SetDesiredJointTorque(tau) # Call logger if LOGGING: logger.sample(device, qualisys=qc, estimator=estimator) # Send command to the robot device.SendCommand(WaitEndOfCycle=True) if ((device.cpt % 100) == 0): device.Print() # Gepetto GUI if k > 0: pos = np.array(estimator.data.oMf[26].translation).ravel() q_viewer[0:3] = np.array([-pos[0], -pos[1], estimator.FK_h]) # Position q_viewer[3:7] = estimator.q_FK[3:7, 0] # Orientation q_viewer[7:] = estimator.q_FK[7:, 0] # Encoders v.display(q_viewer) t += DT k += 1 # **************************************************************** # Whatever happened we send 0 torques to the motors. device.SetDesiredJointTorque([0] * nb_motors) device.SendCommand(WaitEndOfCycle=True) if device.hardware.IsTimeout(): print("Masterboard timeout detected.") print( "Either the masterboard has been shut down or there has been a connection issue with the cable/wifi." ) # Shut down the interface between the computer and the master board device.hardware.Stop() # Save the logs of the Logger object if LOGGING: logger.saveAll() if SIMULATION and enable_pyb_GUI: # Disconnect the PyBullet server (also close the GUI) device.Stop() print("End of script") quit()
def mcapi_playback(name_interface, filename): device = Solo12(name_interface,dt=DT) qc = QualisysClient(ip="140.93.16.160", body_id=0) logger = Logger(device, qualisys=qc) nb_motors = device.nb_motors q_viewer = np.array((7 + nb_motors) * [0.,]) # Load contactsequence from file: cs = ContactSequence(0) cs.loadFromBinary(filename) # extract (q, dq, tau) trajectories: q_t = cs.concatenateQtrajectories() # with the freeflyer configuration dq_t = cs.concatenateDQtrajectories() # with the freeflyer configuration ddq_t = cs.concatenateDDQtrajectories() # with the freeflyer configuration tau_t = cs.concatenateTauTrajectories() # joints torques # Get time interval from planning: t_min = q_t.min() t_max = q_t.max() print("## Complete duration of the motion loaded: ", t_max - t_min) # Sanity checks: assert t_min < t_max assert dq_t.min() == t_min assert ddq_t.min() == t_min assert tau_t.min() == t_min assert dq_t.max() == t_max assert ddq_t.max() == t_max assert tau_t.max() == t_max assert q_t.dim() == 19 assert dq_t.dim() == 18 assert ddq_t.dim() == 18 assert tau_t.dim() == 12 num_steps = ceil((t_max - t_min) / DT) + 1 q_mes_t = np.zeros([8, num_steps]) v_mes_t = np.zeros([8, num_steps]) q_des_t = np.zeros([8, num_steps]) v_des_t = np.zeros([8, num_steps]) tau_des_t = np.zeros([8, num_steps]) tau_send_t = np.zeros([8, num_steps]) tau_mesured_t = np.zeros([8, num_steps]) q_init = config_12_to_8(q_t(t_min)[7:]) device.Init(calibrateEncoders=True, q_init=q_init) t = t_min put_on_the_floor(device, q_init) #CONTROL LOOP *************************************************** t_id = 0 while ((not device.hardware.IsTimeout()) and (t < t_max)): # q_desired = config_12_to_8(q_t(t)[7:]) # remove freeflyer # dq_desired = config_12_to_8(dq_t(t)[6:]) # remove freeflyer # tau_desired = config_12_to_8(tau_t(t)) device.UpdateMeasurment() # tau = compute_pd(q_desired, dq_desired, tau_desired, device) # Parameters of the PD controller KP = 4. KD = 0.05 KT = 1. tau_max = 3. * np.ones(12) # Desired position and velocity for this loop and resulting torques q_desired, v_desired = test_solo12(t) pos_error = q_desired.ravel() - actuators_pos[:] vel_error = v_desired.ravel() - actuators_vel[:] tau = KP * pos_error + KD * vel_error tau = 0.0 * np.maximum(np.minimum(tau, tau_max), -tau_max) # Send desired torques to the robot device.SetDesiredJointTorque(tau) # store desired and mesured data for plotting q_des_t[:, t_id] = q_desired v_des_t[:, t_id] = dq_desired q_mes_t[:, t_id] = device.q_mes v_mes_t[:, t_id] = device.v_mes tau_des_t[:, t_id] = tau_desired tau_send_t[:, t_id] = tau tau_mesured_t[: , t_id] = device.torquesFromCurrentMeasurment # call logger # logger.sample(device, qualisys=qc) device.SendCommand(WaitEndOfCycle=True) if ((device.cpt % 100) == 0): device.Print() q_viewer[3:7] = device.baseOrientation # IMU Attitude q_viewer[7:] = device.q_mes # Encoders t += DT t_id += 1 #**************************************************************** # Whatever happened we send 0 torques to the motors. device.SetDesiredJointTorque([0]*nb_motors) device.SendCommand(WaitEndOfCycle=True) if device.hardware.IsTimeout(): print("Masterboard timeout detected.") print("Either the masterboard has been shut down or there has been a connection issue with the cable/wifi.") device.hardware.Stop() # Shut down the interface between the computer and the master board # Save the logs of the Logger object # logger.saveAll() # Plot the results times = np.arange(t_min, t_max + DT, DT) plot_mes_des_curve(times, q_mes_t, q_des_t, "Joints positions", "joint position") plot_mes_des_curve(times, v_mes_t, v_des_t, "Joints velocities", "joint velocity") plot_mes_des_curve(times, tau_send_t, tau_des_t, "Joints torque", "Nm") current_t = np.zeros([8, num_steps]) for motor in range(device.nb_motors): current_t[device.motorToUrdf[motor], :] = tau_send_t[device.motorToUrdf[motor], :] / device.jointKtSigned[motor] plot_mes_des_curve(times, current_t, title="Motor current", y_label="A") tracking_pos_error = q_des_t - q_mes_t plot_mes_des_curve(times, tracking_pos_error, title="Tracking error") plot_mes_des_curve(times, tau_mesured_t, title="Torque mesured from current", y_label="nM") current_mesured_t = np.zeros([8, num_steps]) for motor in range(device.nb_motors): current_mesured_t[device.motorToUrdf[motor], :] = tau_mesured_t[device.motorToUrdf[motor], :] / device.jointKtSigned[motor] plot_mes_des_curve(times, current_mesured_t, title="Measured motor current", y_label="A") plt.show()
def mcapi_playback(name_interface): """Main function that calibrates the robot, get it into a default waiting position then launch the main control loop once the user has pressed the Enter key Args: name_interface (string): name of the interface that is used to communicate with the robot """ device = Solo12(name_interface, dt=DT) nb_motors = device.nb_motors q_viewer = np.array((7 + nb_motors) * [ 0., ]) v = viewerClient() v.display(q_viewer) # Default position after calibration q_init = np.zeros(12) # Calibrate encoders device.Init(calibrateEncoders=True, q_init=q_init) # Wait for Enter input before starting the control loop put_on_the_floor(device, q_init) # CONTROL LOOP *************************************************** t = 0.0 t_max = 300.0 while ((not device.hardware.IsTimeout()) and (t < t_max)): device.UpdateMeasurment() # Retrieve data from IMU and Motion capture # Zero desired torques tau = np.zeros(12) # Set desired torques for the actuators device.SetDesiredJointTorque(tau) # Send command to the robot device.SendCommand(WaitEndOfCycle=True) if ((device.cpt % 100) == 0): device.Print() # Gepetto GUI q_viewer[3:7] = device.baseOrientation # IMU Attitude q_viewer[7:] = device.q_mes # Encoders q_viewer[2] = 0.5 v.display(q_viewer) t += DT # **************************************************************** # Whatever happened we send 0 torques to the motors. device.SetDesiredJointTorque([0] * nb_motors) device.SendCommand(WaitEndOfCycle=True) if device.hardware.IsTimeout(): print("Masterboard timeout detected.") print( "Either the masterboard has been shut down or there has been a connection issue with the cable/wifi." ) device.hardware.Stop( ) # Shut down the interface between the computer and the master board
def mcapi_playback(name_interface): """Main function that calibrates the robot, get it into a default waiting position then launch the main control loop once the user has pressed the Enter key Args: name_interface (string): name of the interface that is used to communicate with the robot """ device = Solo12(name_interface, dt=DT) # qc = QualisysClient(ip="140.93.16.160", body_id=0) # QualisysClient # logger = Logger(device, qualisys=qc) # Logger object nb_motors = device.nb_motors # Default position after calibration q_init = np.array( [0.0, 0.8, -1.6, 0, 0.8, -1.6, 0, -0.8, 1.6, 0, -0.8, 1.6]) # Calibrate encoders device.Init(calibrateEncoders=True, q_init=q_init) # Wait for Enter input before starting the control loop put_on_the_floor(device, q_init) # CONTROL LOOP *************************************************** t = 0.0 t_max = t_switch[-1] # Parameters of the PD controller KP = 2. KD = 0.05 tau_max = 5. * np.ones(12) while ((not device.hardware.IsTimeout()) and (t < t_max)): device.UpdateMeasurment() # Retrieve data from IMU and Motion capture # Desired position and velocity for this loop and resulting torques q_desired, v_desired = demo_solo12(t) pos_error = q_desired.ravel() - device.q_mes.ravel() vel_error = v_desired.ravel() - device.v_mes.ravel() tau = KP * pos_error + KD * vel_error tau = np.maximum(np.minimum(tau, tau_max), -tau_max) # Set desired torques for the actuators device.SetDesiredJointTorque(tau) # Call logger # logger.sample(device, qualisys=qc) # Send command to the robot device.SendCommand(WaitEndOfCycle=True) if ((device.cpt % 100) == 0): device.Print() t += DT # **************************************************************** # Whatever happened we send 0 torques to the motors. device.SetDesiredJointTorque([0] * nb_motors) device.SendCommand(WaitEndOfCycle=True) if device.hardware.IsTimeout(): print("Masterboard timeout detected.") print( "Either the masterboard has been shut down or there has been a connection issue with the cable/wifi." ) device.hardware.Stop( ) # Shut down the interface between the computer and the master board
def mcapi_playback(name_interface): """Main function that calibrates the robot, get it into a default waiting position then launch the main control loop once the user has pressed the Enter key Args: name_interface (string): name of the interface that is used to communicate with the robot """ name_replay = "/home/odri/git/thomasCbrs/log_eval/test_3/06_nl/" # name_replay = "/home/odri/git/thomasCbrs/log_eval/vmax_nl/" # replay_q = np.loadtxt(name_replay + "_q.dat", delimiter=" ") # replay_v = np.loadtxt(name_replay + "_v.dat", delimiter=" ") # replay_tau = np.loadtxt(name_replay + "_tau.dat", delimiter=" ") qtsid_full = np.load(name_replay + "qtsid.npy" , allow_pickle = True) vtsid_full = np.load(name_replay + "vtsid.npy" , allow_pickle = True) tau_ff = np.load(name_replay + "torques_ff.npy" , allow_pickle = True) replay_q = qtsid_full[7:,:].transpose() replay_v = vtsid_full[6:,:].transpose() replay_tau = tau_ff.transpose() N_SIMULATION = replay_q.shape[0] # Default position after calibration # q_init = replay_q[0, 1:] q_init = replay_q[0, :] if SIMULATION: device = PyBulletSimulator() qc = None else: device = Solo12(name_interface, dt=DT) qc = QualisysClient(ip="140.93.16.160", body_id=0) if LOGGING: logger = Logger(device, qualisys=qc, logSize=N_SIMULATION) # Number of motors nb_motors = device.nb_motors # Initiate communication with the device and calibrate encoders if SIMULATION: device.Init(calibrateEncoders=True, q_init=q_init, envID=0, use_flat_plane=True, enable_pyb_GUI=True, dt=DT) else: device.Init(calibrateEncoders=True, q_init=q_init) # Wait for Enter input before starting the control loop put_on_the_floor(device, q_init) # CONTROL LOOP *************************************************** t = 0.0 t_max = (N_SIMULATION-1) * DT i = 0 P = 7 * np.ones(12) D = 0.5 * np.ones(12) q_des = np.zeros(12) v_des = np.zeros(12) tau_ff = np.zeros(12) while ((not device.hardware.IsTimeout()) and (t < t_max)): device.UpdateMeasurment() # Retrieve data from IMU and Motion capture # Set desired quantities for the actuators device.SetDesiredJointPDgains(P, D) # device.SetDesiredJointPosition(replay_q[i, 1:]) # device.SetDesiredJointVelocity(replay_v[i, 1:]) # device.SetDesiredJointTorque(replay_tau[i, 1:]) device.SetDesiredJointPosition(replay_q[i, :]) device.SetDesiredJointVelocity(replay_v[i, :]) device.SetDesiredJointTorque(replay_tau[i, :]) # Call logger if LOGGING: logger.sample(device, qualisys=qc) # Send command to the robot device.SendCommand(WaitEndOfCycle=True) if ((device.cpt % 1000) == 0): device.Print() t += DT i += 1 # DAMPING TO GET ON THE GROUND PROGRESSIVELY ********************* t = 0.0 t_max = 2.5 while ((not device.hardware.IsTimeout()) and (t < t_max)): device.UpdateMeasurment() # Retrieve data from IMU and Motion capture # Set desired quantities for the actuators device.SetDesiredJointPDgains(np.zeros(12), 0.1 * np.ones(12)) device.SetDesiredJointPosition(np.zeros(12)) device.SetDesiredJointVelocity(np.zeros(12)) device.SetDesiredJointTorque(np.zeros(12)) # Send command to the robot device.SendCommand(WaitEndOfCycle=True) if ((device.cpt % 1000) == 0): device.Print() t += DT # FINAL SHUTDOWN ************************************************* # Whatever happened we send 0 torques to the motors. device.SetDesiredJointTorque([0]*nb_motors) device.SendCommand(WaitEndOfCycle=True) if device.hardware.IsTimeout(): print("Masterboard timeout detected.") print("Either the masterboard has been shut down or there has been a connection issue with the cable/wifi.") device.hardware.Stop() # Shut down the interface between the computer and the master board # Save the logs of the Logger object if LOGGING: logger.saveAll() print("Log saved") if SIMULATION: # Disconnect the PyBullet server (also close the GUI) device.Stop() print("End of script") quit()
def example_script(name_interface, legs_clib_path, shd_clib_path): device = Solo12(name_interface, dt=0.001) nb_motors = device.nb_motors LOGGING = False VIEWER = False qc = None if LOGGING: # Initialize logger qc = QualisysClient(ip="140.93.16.160", body_id=0) # ?? logger = Logger(device, qualisys=qc, logSize=50000) #### Set ref. traj. PD parameters ref_traj_KP = 0 ref_traj_KV = 0 active_dof = [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11] #### Set collision avoidance parameters legs_threshold = 0.05 legs_kp = 20. legs_kv = 0.0 nb_legs_pairs = 20 #### Shoulder collision parameters shd_threshold = 0.2 shd_kp = 3. shd_kv = 0. #### Reference traj. parameters q_ref_list = '###.npy' dq_ref_list = '###.npy' traj_KP = 1 * np.ones(12) traj_KP[:] = 0. traj_KV = 0 * np.ones(12) q_init = q_ref_list[0][7:] traj_counter = 0 ### Emergency behavior switches q_bounds = [-4, 4] vq_max = 20.0 tau_q_max = 1.0 # Load the specified compiled C library cCollFun = CDLL(legs_clib_path) nnCCollFun = CDLL(shd_clib_path) # Initialize emergency behavior trigger var. emergencyFlag = 0 # Initialize viewer if VIEWER: viewer_coll = viewerClient( nb_legs_pairs, 3, legs_threshold, shd_threshold, urdf= "/home/ada/git/tnoel/solopython/coll_avoidance_modules/urdf/solo12_simplified.urdf", modelPath= "/home/ada/git/tnoel/solopython/coll_avoidance_modules/urdf") device.Init(calibrateEncoders=True, q_init=q_init) put_on_the_floor(device, q_init) #CONTROL LOOP *************************************************** tau_q = np.zeros(nb_motors) tau_PD = np.zeros(nb_motors) while ((not device.hardware.IsTimeout()) and (clock() < 120) and emergencyFlag == 0): device.UpdateMeasurment() tau_q[:] = 0. tau_PD[:] = 0. # Compute PD to follow reference traj. curr_q_ref = q_ref_list[traj_counter][7:] curr_dq_ref = dq_ref_list[traj_counter][6:] tau_PD = compute_pd(q_desired, v_desired, KP, KD, device) traj_counter += 1 # Compute collisions distances and jacobians from the C lib. c_results = getLegsCollisionsResults(device.q_mes, cCollFun, nb_motors, nb_legs_pairs, witnessPoints=True) c_dist_legs = getLegsDistances(c_results, nb_motors, nb_legs_pairs, witnessPoints=True) c_Jlegs = getLegsJacobians(c_results, nb_motors, nb_legs_pairs, witnessPoints=True) c_wPoints = getLegsWitnessPoints(c_results, nb_motors, nb_legs_pairs) ### Get results from C generated code (shoulder neural net) #c_shd_dist, c_shd_jac = getAllShouldersCollisionsResults(device.q_mes, nnCCollFun, 2, offset=0.08) # 2D neural net c_shd_dist, c_shd_jac = getAllShouldersCollisionsResults( device.q_mes, nnCCollFun, 3, offset=0.11) #offset with 3 inputs: 0.18 (small), 0.11 (large)" # Compute collision avoidance torque tau_legs = computeRepulsiveTorque(device.q_mes, device.v_mes, c_dist_legs, c_Jlegs, legs_threshold, legs_kp, legs_kv, opposeJacIfNegDist=True) tau_shd = computeRepulsiveTorque(device.q_mes, device.v_mes, c_shd_dist, c_shd_jac, shd_threshold, shd_kp, shd_kv, opposeJacIfNegDist=False) tau_q = 1 * tau_legs + 1 * tau_shd # Set the computed torque as command tau_command = tau_q + tau_PD device.SetDesiredJointTorque(0 * tau_command) # Check the condition for triggering emergency behavior emergencyFlag = max( emergencyFlag, emergencyCondition(device.q_mes, device.v_mes, tau_command, q_bounds, vq_max, tau_q_max)) # Call logger if LOGGING: logger.sample(device, qualisys=qc) if VIEWER: viewer_coll.display( np.concatenate(([0, 0, 0, 0, 0, 0, 0], device.q_mes)), c_dist_legs, c_shd_dist, c_wPoints, tau_legs, tau_shd) device.SendCommand(WaitEndOfCycle=True) if ((device.cpt % 100) == 0): device.Print() print('Avoid. torque') print(tau_q) print('PD torque') print(tau_PD) #**************************************************************** print("Emergency : {}".format(emergencyFlag)) # Whatever happened we send 0 torques to the motors. device.SetDesiredJointTorque([0] * nb_motors) device.SendCommand(WaitEndOfCycle=True) # Save the logs of the Logger object if LOGGING: logger.saveAll() print("Log saved") if device.hardware.IsTimeout(): print("Masterboard timeout detected.") print( "Either the masterboard has been shut down or there has been a connection issue with the cable/wifi." ) device.hardware.Stop( ) # Shut down the interface between the computer and the master board